perm filename ARMSOL.PAL[AL,HE]11 blob
sn#575134 filedate 1981-03-30 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00058 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00006 00002 ARMSOL - SUITE OF PROGRAMS INVOLVING ARM KINEMATICS
C00009 00003 DEFINITIONS FOR USING THE MATRICES AND VECTORS:
C00011 00004 SOLVE - COMPUTES JOINT ANGLES GIVEN A TRANSFORM
C00014 00005 ["SOLVE" - CHECK IF ARM OR HAND]
C00018 00006 ["SOLVE" - JOINT 1]
C00021 00007 ["SOLVE" - JOINTS 2 & 3]
C00023 00008 ["SOLVE" - JOINT 5]
C00026 00009 ["SOLVE" - DEGENERATE SOLUTION AND JOINT 4]
C00030 00010 ["SOLVE" - JOINT 6]
C00033 00011 ["SOLVE" LOCAL STORAGE AREA]
C00035 00012 ---> PUMSOL <--- THE PUMA ARM SOLUTION
C00044 00013 ["PUMSOL" - THE MAIN PART]
C00050 00014 ["PUMSOL" - GETSOL]
C00054 00015 ["PUMSOL" - SSETUP]
C00058 00016 ["PUMSOL" - ROTATION ROUTINES]
C00061 00017 ["PUMSOL" - FINDS]
C00066 00018 ["PUMSOL" - GET1]
C00070 00019 ["PUMSOL" - GET3]
C00074 00020 ["PUMSOL" - GET2]
C00078 00021 ["PUMSOL" - GET4]
C00080 00022 ["PUMSOL" - GET5]
C00082 00023 ["PUMSOL" - GET6]
C00085 00024 ["PUMSOL" - FIX4]
C00088 00025 ["PUMSOL" - OFSETS]
C00090 00026 ["PUMSOL" - ANGLE MANIPULATION ROUTINES]
C00094 00027 ["PUMSOL" - VECTOR MANIPULATION ROUTINES]
C00098 00028 ["PUMSOL" - LOCAL DATA]
C00100 00029 UPDATE - COMPUTES THE ARM TRANSFORM GIVEN THE JOINT ANGLES
C00106 00030 ["UPDATE"- HAND OR T6]
C00109 00031 ---> PUMUPD < --- THE PUMA UPDATE ROUTINE
C00115 00032 [CONTINUATION OF PUMUPD: THE MAIN PART]
C00116 00033 [CONTINUATION OF PUMUPD: DOTRIG]
C00118 00034 [CONTINUATION OF PUMUPD: GETN]
C00120 00035 [CONTINUATION OF PUMUPD: GETO]
C00122 00036 [CONTINUATION OF PUMUPD: GETA]
C00124 00037 [CONTINUATION OF PUMUPD: GETP]
C00126 00038 [CONTINUATION OF PUMUPD: GETXY]
C00127 00039 [CONTINUATION OF PUMUPD: OFSETW]
C00130 00040 [CONTINUATION OF PUMUPD: LOCAL DATA AND CONSTANTS]
C00131 00041 JACOB - COMPUTES JACOBIAN MATRIX
C00132 00042 ["JACOB" - FORM CT0,CT3,CT6]
C00135 00043 ["JACOB" - POSITION VECTORS, AXES OF ROTATION]
C00138 00044 ["JACOB" - CROSS PRODUCTS, CLEAN UP]
C00140 00045 T0336 - COMPUTES TRANSFORMATION MATRICIES FOR JTS 1-3 AND 4-6
C00143 00046 T6330 - COMPUTES TRANSFORMATION MATRICIES FOR JTS 6-4 AND 3-0
C00145 00047 R03 - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 1-3
C00147 00048 R36 - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 4-6
C00150 00049 INVERT - INVERTS A TRANSFORMATION STORED BY COLUMNS
C00151 00050 MATMUL - PERFORMS THE OPERATION T0 ← T1*T2
C00154 00051 MULT - PERFORMS THE OPERATION A0 ← A1*A2
C00156 00052 CROSS - VECTOR CROSS PRODUCT OPERATION V0 ← V1 X V2
C00158 00053 LOCAL STORAGE AREA
C00160 00054 PHYSICAL CONSTANTS FOR YELLOW AND BLUE ARMS
C00161 00055 [YELLOW ARM TABLE OF CONSTANTS]
C00162 00056 [BLUE ARM TABLE OF CONSTANTS]
C00163 00057 [GREEN & RED ARM TABLE OF CONSTANTS]
C00164 00058 PUMA CONSTANTS AND MISC. DATA FOR PUMA ROUTINES
C00166 ENDMK
C⊗;
;ARMSOL - SUITE OF PROGRAMS INVOLVING ARM KINEMATICS
.TITLE ARMSOL
;THE FOLLOWING PROGRAMS ARE USED TO COMPUTE THE JOINT ANGLES FOR THE
;SCHEINMAN MANIPULATORS GIVEN A REQUIRED TRANSFORM OR VISE VERSA.
;ALSO INCLUDED IS A PROGRAM FOR COMPUTING THE REACTION TORQUES AT THE
;JOINTS FOR A GIVEN FORCE AND MOMENT COUPLE AT THE HAND.
;THE THEORETICAL BASIS FOR THE KINEMATIC SOLUTIONS IMPLEMENTED IN
;THESE PROGRAMS IS PRESENTED IN STANFORD ARTIFICIAL INTELLIGENCE
;LABORATORY HAND/EYE GROUP PROGRESS REPORT, DEC 1975 THRU JULY 1976.
;WHEREVER POSSIBLE, THE VARIABLE NAMES USED IN THAT DOCUMENT AND IN
;THE FOLLOWING PROGRAMS ARE THE SAME.
;THE FOLLOWING MECHANISM BITS INDICATE WHICH PHYSICAL DEVICE IS BEING
;USED:
YELARM ==1 ;YELLOW ARM, NOT INCLUDING HAND
YELHND ==2 ;YELLOW HAND
BLUARM ==4 ;BLUE ARM, NOT INCLUDING HAND
BLUHND ==10 ;BLUE HAND
GRNARM ==100 ;GREEN ARM (PUMA NEAR DOOR)
GRNHND ==200 ;GREEN HAND
REDARM ==400 ;RED ARM (PUMA NEAR WALL)
REDHND ==1000 ;RED HAND
;THE FOLLOWING BITS ARE USED BY THE FORCE SENSING ROUTINES TO INDICATE
;IF THE FORCE TRANSFORMATION IS IN TABLE OR HAND COORDINATES.
FTABLE ==400 ;TABLE COORDINATES
FHAND ==0 ;HAND COORDINATE SYSTEM
.MACRO SQRFIX X ;IF X IS A SMALL NEGATIVE NUMBER, MAKE IT ZERO.
CHECKF X ;IS IT POSITIVE OR ZERO?
BGE .+16 ;IF SO, EXIT
COMPF TINYNO,X ;IS TINY# > X?
BGT .+6
CLRF X ;IT'S DUE TO ROUNDING, PROBABLY. CLEAR IT.
.ENDM
; DEFINITIONS FOR USING THE MATRICES AND VECTORS:
;Transformation definitions for the Stanford Arm routines
T11==0
T21==4
T31==10
T12==14
T22==20
T32==24
T13==30
T23==34
T33==40
T14==44
T24==50
T34==54
;Transformation definitions for the PUMA routines
NX == 0 ;First row of the position matrix (see PUMSOL for details)
NY == 4
NZ == 10
OX == 14 ;Second row
OY == 20
OZ == 24
AX == 30 ;Third row
AY == 34
AZ == 40
PX == 44 ;Fourth row
PY == 50
PZ == 54
TH1 == 0 ;DEFINITIONS FOR A 6-VECTOR OF JOINT ANGLES POINTERS
TH2 == 2
TH3 == 4
TH4 == 6
TH5 == 8.
TH6 == 10.
XCOORD == 0 ;COMPONENTS OF 3-VECTORS
YCOORD == 4
ZCOORD == 10
FARBIT==400 ;This bit is set in R0 if a close solution was not found.
;SOLVE - COMPUTES JOINT ANGLES GIVEN A TRANSFORM
;GIVEN A TRANSFORM "T", THE REQUIRED JOINT ANGLES ARE DETERMINED IN DEGREES.
;IF ANY JOINT ANGLE IS OUTSIDE OF THE PERMITTED RANGE OF MOVEMENT, THE STOP
;LIMIT ANGLE CLOSEST TO THE PREVIOUS JOINT ANGLES COMPUTED IS RETURNED.
;IN THE DETERMINATION OF JOINT ANGLE 4, IF THE REQUIRED CHANGE IN ANGLE IS
;GREATER THAN 90 DEGRESS AND LESS THAN 180, THE HAND IS FLIPPED OVER AND THE
;JOINT ANGLE CHANGE BECOMES THETA = THETA-90. ON COMPLETION OF EXECUTION,
;REGISTER R0 CONTAINS BITS INDICATING THOSE JOINTS FOR WHICH NO SOLUTION
;COULD BE FOUND WITHIN THE RANGE OF JOINT MOTION. A SAMPLE CALLING SEQUENCE
;TO "SOLVE" FOLLOWS:
;
; MOV #T,R0 ;LOAD ADDRESS OF TRANSFORM "T"
; MOV #THETA,R1 ;PTR TO A TABLE CONTAINING POINTERS TO THE
; ; JOINT ANGLES FOR ALL EXISTING ARM JOINTS,
; ; I.E., THE TABLE MUST HAVE 28 ELEMENTS,
; ; 7 FOR EACH ARM.
; MOV #MECHSM,R2 ;CONSTANT INDICATING WHICH ARM TO USE
; JSR PC,SOLVE ;CALLED USING PC
; TST R0 ;CHECK FOR NUMBER OF NON-EXACT SOLUTIONS
;
;IF THE BLUE OR YELLOW HAND IS SPECIFIED AS THE MECHANISM, THIS ROUTINE
;PUTS THE VALUE POINTED TO BY "T" INTO ITS APPROPRIATE SPOT IN THE "THETA"
;ARRAY. ALL NUMBERS SHOULD BE IN SINGLE PRECISION FLOATING POINT
;REPRESENATATION.
;EXECUTION TIME: 2200 MICRO SECONDS
;REGISTERS USED:
;
; R0, R1 PASS ARGUMENTS AND ARE ALTERED BY "SOLVE".
; R2 PASSES ARGUMENTS AND IS UNALTERED.
; AC0, AC1, AC2, AND AC3 ARE GARBAGED
;THE OUT OF RANGE BITS THAT ARE RETURNED IN R0 ARE DEFINED AS FOLLOWS:
JT1== 1 ;JOINT 1
JT2== 2 ; " 2 (NOT USED FOR STANFORD ARMS)
JT3== 4 ; " 3
JT4== 10 ; " 4
JT5== 20 ; " 5
JT6== 40 ; " 6
; ["SOLVE" - CHECK IF ARM OR HAND]
CODE
SOLVE: CLR EXACTS ;ASSUME EXACT SOLUTION FOUND
MOV R5,-(SP) ;SAVE REGISTER
BIT #BLUHND+YELHND+GRNHND+REDHND,R2 ;CHECK IF A HAND IS SPECIFIED
BEQ 1$ ;BRANCH IF NOT
; CODE TO HANDLE HANDS
; NOT SURE WHAT TO DO HERE FOR THE PUMA
LDF (R0),AC0 ;GET THE REQUESTED HAND OPENING
ADD #12.,R1 ;POINT TO THE YELLOW HAND CELL
BIT #BLUHND,R2 ;CHECK WHICH HAND
BEQ .+6
ADD #14.,R1 ;POINT TO BLUE HAND
STF AC0,@(R1) ;SAVE THE HAND OPENING
JMP SOLDNE ;AND EXIT
; CODE TO HANDLE ARMS
1$: BIT #GRNARM+REDARM,R2 ;IS IT A PUMA??
BEQ 10$ ;IF NOT, GO INTO STANFORD ARM SOL'N
MOV #GCON,R5 ;ASSUME GREEN ARM & SET R5 TO CONSTANT TABLE
ADD #32.,R1 ;POINT TO GREEN ARM ANGLES.
BIT #REDARM,R2 ;WAS IT ACTUALLY THE RED ARM?
BEQ 20$ ;IF NOT, BR
MOV #RCON,R5 ;SET R5 TO RED ARM CONSTANTS
ADD #14.,R1 ; AND BUMP R1 TO RED ARM ANGLES.
20$: JSR PC,PUMSOL ;GET PUMA SOLUTION
POP R5 ;POP R5 FROM STACK
RTS PC ;RETURN
10$: MOV #YCON,R5 ;MUST BE STANFORD ARM. GET PTR TO YELLOW CONSTANTS
BIT #YELARM,R2 ;SWITCH CONSTANTS IF BLUE ARM
BNE 2$
MOV #BCON,R5
ADD #14.,R1 ;POINT TO BLUE ARM JT. ANGLES
;COMPUTE POSITION OF THE END OF THE BOOM: TX,TY,TZ. ALSO COMPUTE
;SQRT(TX**2+TY**2-S2**2) FOR LATER
2$: LDF S6(R5),AC1 ;GET OFFSET OF JT. 6: S6
NEGF AC1
LDF T13(R0),AC0 ;TX ← T14 - T13*S6 - X BASE POS.
MULF AC1,AC0
SUBF BASEX(R5),AC0
ADDF T14(R0),AC0
STF AC0,TX ;SAVE TX
MULF AC0,AC0 ;AC0 ← TX**2
LDF T23(R0),AC2 ;TY ← T24 - T23*S6 - Y BASE COORD.
MULF AC1,AC2
SUBF BASEY(R5),AC2
ADDF T24(R0),AC2
STF AC2,TY ;SAVE TY
MULF AC2,AC2 ;AC0 ← TX**2 + TY**2
ADDF AC2,AC0
MULF T33(R0),AC1 ;TZ ← T34 - T33*S6
ADDF T34(R0),AC1
STF AC1,TZ ;SAVE TZ
SUBF S22(R5),AC0 ;AC0 ← TX**2 + TY**2 - S2**2
CFCC ;CHECK IF RESULT > 0
BGT 3$
CLRF AC0 ;IF <0 SET TO 0 AND INDICATE ERROR
BIS #JT1,EXACTS ;THIS WILL CAUSE JT 1 TO BE WRONG
3$: JSR PC,SQRTF
NEGF AC0
STF AC0,SQRTXY ;SAVE -SQRT(TX**2+TY**2-S2**29
; ["SOLVE" - JOINT 1]
;COMPUTE JOINT 1 ANGLE
SUBF TX,AC0 ;TAN TH1/2= (-TX-(TX↑2+TY↑2-S2↑2)↑.5)/(S2+TY)
LDF TY,AC1 ;INDETERMINANT? IS S2+TY = 0 ?
ADDF S2(R5),AC1
STF AC1,AC2 ;COMPARE TO .001
ABSF AC2
CMPF C001,AC2
CFCC
BLT 5$ ;BRANCH IF DENOMINATOR NOT ZERO
TST TX ;ELSE CHECK IF INDETERMINATE OR SPECIAL CASE
BLT 4$ ;BRANCH IF INDETERMINATE FORM OF EQU. NECESSARY
LDF C180,AC0 ;ELSE THETA 1 = 180 DEGREES
BR JT1CHK ;NOW GO CHECK LIMITS
4$: LDF TY,AC0 ;TAN TH1/2 = TY/TX
LDF TX,AC1
5$: TSTF AC1 ;-90 < TH1/2 < 90
CFCC
BGE JT1CPU
NEGF AC0
NEGF AC1
JT1CPU: JSR PC,ATAN2 ;COMPUTE THETA 1/2
MULF #40400,AC0 ;THETA 1
JT1CHK: CMP #YELARM,R2 ;CHECK IF YELLOW OR BLUE ARM
BNE 1$ ;BRANCH IF BLUE ARM
CMPF STOP1+4(R5),AC0 ;IF YELLOW, CHECK IF LESS THAN +90 DEG
CFCC
BGE .+6 ;SKIP IF LESS THAN MAX STOP LIMIT
SUBF C360,AC0 ;ELSE SUBTRACT 360 DEGREES
CMPF STOP1(R5),AC0 ;COMPARE TO MIN STOP LIMIT
CFCC
BLE J1OK ;BRANCH IF WITHIN STOP LIMITS
BR 2$
1$: CMPF STOP1(R5),AC0 ;IF BLUE, CHECK IF GREATER THAN -90 DEG
CFCC
BLE .+6 ;SKIP IF GREATER THAN
ADDF C360,AC0 ;ELSE ADD 360 DEGREES
CMPF STOP1+4(R5),AC0 ;COMPARE ANGLE TO MAX STOP LIMIT
CFCC
BGE J1OK ;BRANCH IF WITHIN STOP LIMITS
2$: BIS #JT1,EXACTS ;INDICATE NO EXACT SOLUTION
LDF STOP1(R5),AC0 ;USE CLOSEST STOP LIMIT
LDF MIDDY1(R5),AC1 ;MID-RANGE ANGLE
CMPF @(R1),AC1 ;COMPARE TO PREVIOUS
CFCC
BLE J1OK
LDF STOP1+4(R5),AC0 ;USE MAXIMUM IF CLOSER
J1OK: STF AC0,@(R1)+ ;SEND OFF NEW JOINT 1 ANGLE
JSR PC,SNCOS ;COMPUTE SIN/COS THETA 1 FOR LATER
STF AC0,ST1
STF AC1,CT1
; ["SOLVE" - JOINTS 2 & 3]
;COMPUTE JOINT 2 ANGLE
LDF SQRTXY,AC0 ;TAN TH2 = -(TX↑2+TY↑2-S2↑2)↑.5/(TZ-S1)
LDF TZ,AC1
SUBF S1(R5),AC1
JSR PC,ATAN2
STF AC0,@(R1)+ ;SEND OF NEW JOINT 2 ANGLE. NO STOP LIMIT
; CHECKING SINCE ALL SOLUTIONS WITHIN 0 TO Pi
JSR PC,SNCOS ;COMPUTE SIN/COS OF THETA 2 FOR LATER
SUBF C000,AC0 ;MAKE SURE SINE THETA 2 NOT ZERO
STF AC0,ST2
STF AC1,CT2
;COMPUTE JOINT 3 LENGTH
LDF TX,AC2 ;S3 ← (TX*CT1+TY*ST1)/ST2
MULF CT1,AC2
LDF TY,AC3
MULF ST1,AC3
ADDF AC3,AC2
DIVF AC0,AC2 ;NOW HAVE S3
CMPF STOP3+4(R5),AC2 ;COMPARE TO MAX. JOINT EXTENSION
CFCC
BGE 1$ ;BRANCH IF LESS THAN MAX
LDF STOP3+4(R5),AC2 ;ELSE SUBSTITUE MAX ALLOWABLE EXT.
BR 2$
1$: CMPF STOP3(R5),AC2 ;COMPARE TO MIN. JOINT EXTENSION
CFCC
BLE S3OK ;BRANCH IF IN RANGE
LDF STOP3(R5),AC2 ;ELSE SUBSTITUTE MIN LENGTH
2$: BIS #JT3,EXACTS ;INDICATE EXACT SOLUTION NOT FOUND
S3OK: STF AC2,@(R1)+ ;SEND BACK JOINT 3
; ["SOLVE" - JOINT 5]
;COMPUTE COS/SIN OF THETA 5 AND CHECK FOR DEGENERACY
LDF T13(R0),AC0 ;SIN TH5 = SQRT( (-T33*SN2+(T13*CT1+T23*ST1)*CT2)↑2
MULF CT1,AC0 ; + (T13*ST1-T23*CT1)↑2 )
LDF T23(R0),AC1
MULF ST1,AC1
ADDF AC1,AC0
STF AC0,T13T23 ;SAVE T13*CT1+T23*ST1
MULF CT2,AC0
LDF T33(R0),AC1
MULF ST2,AC1
SUBF AC1,AC0
STF AC0,ST4 ;THIS IS ALMOST SIN(THETA 4)
MULF AC0,AC0
LDF T13(R0),AC1 ;NOW GET T13*ST1-T23*CT1
MULF ST1,AC1
LDF T23(R0),AC2
MULF CT1,AC2
SUBF AC2,AC1
STF AC1,CT4 ;THIS IS ALMOST COS(THETA 4)
MULF AC1,AC1
ADDF AC1,AC0
JSR PC,SQRTF ;NOW HAVE SIN(THETA 5)
LDF T13T23,AC1 ;COS TH5 = T33*CT2+(T13*CT1+T23*ST1)*ST2
MULF ST2,AC1
LDF T33(R0),AC2
MULF CT2,AC2
ADDF AC2,AC1
JSR PC,ATAN2
ABSF AC0 ;WANT +180 NOT -180
TST @2(R1) ;CHECK SIGN OF PREVIOUS THETA 5
BGE .+4
NEGF AC0 ;SET SIGN OF CURRENT ANGLE THE SAME
CMPF STOP5+4(R5),AC0 ;COMPARE TO MAXIMUM STOP LIMIT
CFCC
BGE 1$ ;BRANCH IF ANGLE LESS THAN STOP LIMIT
LDF STOP5+4(R5),AC0 ;ELSE REPLACE ANGLE BY STOP LIMIT
BR 2$
1$: CMPF STOP5(R5),AC0 ;COMPARE ANGLE TO MIN STOP LIMIT
CFCC
BLE JT5OK ;BRANCH IF ANGLE IN RANGE
LDF STOP5(R5),AC0 ;ELSE REPLACE ANGLE BY MIN STOP LIMIT
2$: BIS #JT5,EXACTS ;INDICATE NO EXACT SOLUTION FOUND
JT5OK: STF AC0,@2(R1) ;SEND BACK ANGLE
STF AC0,AC1 ;CHECK FOR DEGENERATE CASE, TH5 = 0
ABSF AC1
CMPF C01,AC1
CFCC
BLT NOTDGN ;BRANCH IF NOT DEGENERATE SOLUTION
; ["SOLVE" - DEGENERATE SOLUTION AND JOINT 4]
;DEGENERATE SOLUTION: TH5 = 0, TH4 REMAINS THE SAME, SPECIAL
;EQUATION FOR TH6
LDF T31(R0),AC0 ;SIN(TH6+TH4)=-T31/ST2
LDF T32(R0),AC1 ;COS(TH6+TH4)=-T32/ST2
JSR PC,ATAN2 ;COMPUTE TH6+TH4
SUBF @(R1)+,AC0 ;TH6 = ATAN -TH4
TST (R1)+
CMPF STOP6+4(R5),AC0 ;MIGHT BE OFF BY 360 DEG
CFCC ;GREATER THAN MAXIMUM STOP LIMIT?
BGE 1$
SUBF C360,AC0 ;YES, SUBTRACT 360 DEGREES
JMP JT6CHK ;GO CHECK IF JT 6 IN RANGE
1$: CMPF STOP6(R5),AC0 ;LESS THAN MINIMUM STOP LIMIT?
CFCC
BLE 2$
ADDF C360,AC0 ;YES, ADD 360 DEGREES
2$: JMP JT6CHK
;NON-DEGENERATE SOLUTION: COMPUTE THETA 4
NOTDGN: LDF ST4,AC0 ;SIN(THETA 4)=-T33*SN2+(T13*CT1+T23*ST1)*CT2/ST5
LDF CT4,AC1 ;COS(THETA 4)=(T13*ST1-T23*CT1)/ST5
TST @2(R1) ;DONT DIVID, JUST CORRECT FOR SIGN(ST5)
BPL 1$
NEGF AC0
NEGF AC1
1$: JSR PC,ATAN2 ;COMPUTE THETA 4
STF AC0,AC1 ;DIFFERENCE BETWEEN OLD AND NEW ANGLE
SUBF @(R1),AC1
CMPF C180,AC1 ;DIFFERENCE > 180 DEG ?
CFCC
BGE 2$ ;BRANCH IF LESS
SUBF C360,AC0 ;ELSE SUBT. 360 DEG - GO THE SHORTEST DIRECTION
SUBF C360,AC1
BR 3$
2$: CMPF CM180,AC1 ;DIFFERENCE > -180 DEG ?
CFCC
BLE 3$ ;BRANCH IF GREATER
ADDF C360,AC0 ;ELSE ADD 360 DEG - GO THE SHORTEST DIRECTION
ADDF C360,AC1
3$: CMPF C90,AC1 ;DIFF. < 90 DEG ?
CFCC
BGE 4$ ;BRANCH IF IT IS
SUBF C180,AC0 ;ELSE GO THE OTHER DIRECTION AND FLIP HAND
BR FLIPJ4
4$: CMPF CM90,AC1 ;DIFF. > -90 DEG ?
CFCC
BLE JT4CHK ;BRANCH IF IT IS
ADDF C180,AC0 ;ELSE GO THE OTHER DIRECTION
FLIPJ4: NEGF @2(R1) ;FLIP HAND OVER
JT4CHK: CMPF STOP4(R5),AC0 ;COMPARE ANGLE TO MIN STOP LIMIT
CFCC
BLE 1$ ;BRANCH IF GREATER THAN MIN
ADDF C180,AC0 ;ELSE ADD 180 DEG AND FLIP HAND
BR 2$
1$: CMPF STOP4+4(R5),AC0 ;COMPARE TO MAX STOP LIMIT
CFCC
BGE JT4OK ;BRANCH IF JT 4 IN RANGE
SUBF C180,AC0 ;ELSE SUBT 180 DEG AND FLIP HAND
2$: NEGF @2(R1)
JT4OK: STF AC0,@(R1)+ ;SEND BACK THE NEW JOINT 4 ANGLE
; ["SOLVE" - JOINT 6]
;COMPUTE THETA 6
LDF ST1,AC2 ;SIN(THETA 6)=((T12*CT1+T22*ST1)*ST2
LDF CT1,AC3 ; +T32*CT2)/ST5
LDF T12(R0),AC0
MULF AC3,AC0
LDF T22(R0),AC1
MULF AC2,AC1
ADDF AC1,AC0
MULF ST2,AC0
LDF T32(R0),AC1
MULF CT2,AC1
ADDF AC1,AC0
LDF T21(R0),AC1 ;COS(THETA 6)=-((T21*ST1+T11*CT1)*ST2
MULF AC2,AC1 ; +T31*CT2)/ST5
LDF (R0),AC2
MULF AC3,AC2
ADDF AC2,AC1
MULF ST2,AC1
LDF T31(R0),AC2
MULF CT2,AC2
TST @(R1) ;DON'T DIVID BY ST5, JUST CORRECT SIGN
ADDF AC2,AC1
BGE 1$
NEGF AC0 ;IF SIN(TH5)<0 USE -SIN(TH6)
BR .+4
1$: NEGF AC1 ;ELSE USE -COS(TH6)
TST (R1)+ ;POINT TO OLD THETA 6
JSR PC,ATAN2 ;COMPUTE THETA 6
JT6CHK: CMPF STOP6(R5),AC0 ;CHECK IF ANGLE GREATER THAN MIN.
CFCC
BLE 1$ ;BRANCH IF GREATER
ADDF C180,AC0 ;ELSE ADD 180 DEG AND FLIP HAND
BR 2$
1$: CMPF STOP6+4(R5),AC0 ;CHECK IF ANGLE LESS THAN MAXIMUM
CFCC
BGE JT6OK ;BRANCH IF JT6 IN RANGE
SUBF C180,AC0 ;ELSE SUBT 180 DEG AND FLIP HAND
2$: STF AC0,@(R1) ;SAVE NEW THETA 6
NEGF @-(R1) ;FLIP HAND BY COMPLEMENTING TH5
LDF @-(R1),AC0 ;ALTER THETA 4 BY 180 DEGREES
ADDF C180,AC0 ;TRY ADDING 180 DEGREES
CMPF STOP4+4(R5),AC0 ;COMPARE TO MAXIMUM
CFCC
BGE 3$ ;BRANCH IF STILL IN RANGE
SUBF C360,AC0 ;ELSE TRY SUBT 180 DEGREES
CMPF STOP4(R5),AC0
CFCC
BLE 3$ ;BRANCH IF JT 4 NOW IN RANGE
LDF STOP4(R5),AC0 ;ELSE JUST USE STOP LIMIT
BIS #JT4,EXACTS ;INDICATE NO EXACT SOLUTION
3$: STF AC0,@(R1) ;SAVE NEW THETA 4
BR .+6
JT6OK: STF AC0,@(R1) ;SAVE NEW THETA 6
;EXIT CLEANLY
SOLDNE: MOV (SP)+,R5 ;RESTORE REGISTERS
MOV EXACTS,R0 ;LEAVE ERROR BITS FOR CALLER
RTS PC
;END OF "SOLVE"
; ["SOLVE" LOCAL STORAGE AREA]
DATA
TX: .BLKW 2 ;LOCATION OF THE END OF THE BOOM
TY: .BLKW 2
TZ: .BLKW 2
SQRTXY: .BLKW 2 ;-SQRT(TX**2+TY**2-S2**2)
T13T23: .BLKW 2 ;T13*CT1+T23*ST1
ST1: .BLKW 2 ;SIN/COS THETA 1
CT1: .BLKW 2
ST2: .BLKW 2 ;SIN/COS THETA 2
CT2: .BLKW 2
ST4: .BLKW 2 ;SIN/COS THETA 4 (ALMOST)
CT4: .BLKW 2
EXACTS: 0 ;INDICATES JTS WITH NON-EXACT SOLUTIONS
;CONSTANTS
C01: .WORD 37314,146315 ; .1000000
C001: .WORD 35603, 11157 ; .1000000@-2
C360: .WORD 42264, 0 ; 360.0000
C90: .WORD 41664, 0 ; 90.00000
CM90: .WORD 141664, 0 ;-90.00000
C180: .WORD 42064, 0 ; 180.0000
CM180: .WORD 142064, 0 ;-180.0000
C000: .WORD 31453,146167 ; .1000000@-7
CODE
; ---> PUMSOL <--- THE PUMA ARM SOLUTION
; PUMSOL does the forward solution for the PUMA: it accepts a required position
; represented by a 4x4 matrix, and returns the 6 joint angles necessary to
; achieve this position. For each position, there correspond four possible
; solutions, since the arm can look like a right or left hand, and the elbow
; can be above or below the rest of the arm. In many cases, one or more or
; these solutions may not be valid because of joint stop limits or because
; the solution would cause the arm to reach into itself. This routine uses
; the current position, which is assumed to be in a vector of 6 joint angles
; pointed to by r2, to generate the solution closest to the current position.
; If none of the four solutions are valid, a bit in r3 is set to indicate
; which joint produced an invalid solution.
; In the case of a 5-degree of freedom arm, such as the PUMA with 5 joints,
; some frames may not be accessible by the manipulator. For example, we
; must constrain the angle of joint 4 to be zero, so that any frame accessible
; will be such that the A vector (which lies along the tool axis) is in a
; vertical plane passing through the desired position (P vector) and tangent
; to the circle of radius D3 with center at (0,0,0).
; To achieve this, we must perform a rotation of the goal frame to make it
; correspond to one of these accessible positions. The method used to rotate
; the goal frame is as follows:
; Call the circle of radius D3 about the origin C. This circle defines an
; infinite set of (vertical) planes, each tangent to the circle C, in which
; the approach vector A must lie in order for the goal frame to be accessible
; by the arm. The goal position (specified by the vector P) defines which one
; of these planes A must lie in: it is the plane passing through P and tangent
; to the circle C. Call the point of tangency (r,s,0). We have
; r1 = [Px*D3 - Py*sqrt(D↑2-D3↑2)] / D↑2 s1 = [Py*D3 + Px*sqrt(D↑2-D3↑2] / D↑2
; r2 = [Px*D3 + Py*sqrt(D↑2-D3↑2)] / D↑2 s1 = [Py*D3 - Px*sqrt(D↑2-D3↑2] / D↑2
; with
; D↑2 = Px↑2 + Py↑2
; These two solutions for (r,s) correspond to the two points of tangency to C.
; We choose for (r,s) the values which make the rotation angle needed the minimum.
; Denote by T the normal vector to the plane. Now there are two choices for T
; also. We choose the one which points in the same general direction as A.
; Let T=(r,s,0) and T'=(-r,-s,0). These are our two choices. If we have
; arccos(A⊗T) < π/2 then assign T ← T'.
; Now let S = A x J, where J is A's projection on the plane we have been talking
; about. We have J = A - (T⊗A)T, and this must be made into a unit vector.
; S must be a unit vector also for the next step. The angle needed to rotate
; the goal frame through is theta = arccos(J⊗A).
; We now get A' and O' from A and O, the desired goal frame vectors, by rotating
; both of them about S by an angle theta, with theta as above. We use
; Rodrigues' formula for rotation of vectors, e.g. to rotate V about S by angle α:
; V' = V cosα + (SxA) sinα + (S⊗A) (1-cosα) S
; This vector is unitized and stored into A and O to be used in the calculations
; of the joint angles.
; ARGUMENTS:
; R0 - Points to the matrix C, which contains 16 floating-point
; (single precision) numbers which represent the desired
; position of the arm. The matrix looks like
; | Nx Ox Ax Px |
; | Ny Oy Ay Py |
; | Nz Oz Az Pz |
; | 0 0 0 1 |
; (N, O and A are the Euler angles: i.e. the X axis of the frame
; points along N, the Y axis points along O, and the Z axis points
; along A; P is the position vector)
; The matrix must be stored in column-major order -- i.e. R0 points
; to Nx, which is followed by Nx, etc.
; R1 - On entry, points to a 6-vector J which contains pointers to
; the current joint angles, which we use to insure the solution
; produced is "close".
; R5 - Points to a table of arm constants which are used to relocate
; the base of the coordinate system. The origin of the PUMA's
; world is at the base of its shoulder, so we need to know the
; offset of this base and subtract it off from the place AL
; wants us to move to. R5 must point to a 3-vector of floating-pt
; numbers (X0,Y0,Z0) which is the location of the PUMA's base.
; RETURNED VALUES:
; R0 - On return, R0 is set to indicate which joints could not be
; solved for, or cleared if a valid solution was found.
; Also, if the "close" solution could not be found, the bit
; FARBIT is set on in R0 to indicate this
; R1 - Unchanged, but the 6 joint angles referenced by R1 have been
; changed to those angles necessary to move the arm to the
; position defined by matrix C. These will be single-precision
; floating numbers, in degrees.
; SAMPLE CALLING SEQUENCE:
; MOV #MATRIX,R0 ;Set up address of cartesian matrix
; MOV #JVECT,R1 ;Set up address of result (joint vector)
; MOV #CONST,R5 ;Pointer to arm constants (ie offset of origin)
; JSR PC,PUMSOL ;Get FORWARD SOLUTION.
; TST R0 ;Check for invalid solution
; REGISTERS USED:
; R0,R1,R5: For passing arguments. R0 & R5 are changed.
; AC0,AC1,AC2,AC3: Garbaged.
; ["PUMSOL" - THE MAIN PART]
PUMSOL: PUSH R2
PUSH R3
PUSH R4 ;save registers we may use
MOV R5,PWHICH ;Save pointer to arm constants
GOSUB SSETUP ;set up arguments - NX,NY,NZ,OX,etc.
CLR FAR ;Assume we found close sol'n
LDF @TH1(R1),AC0 ;SAVE CURRENT JOINT 1 ANGLE
STF AC0,CUR1
LDF @TH2(R1),AC0 ;CURRENT JOINT 2 ANGLE
STF AC0,CUR2
; Rotate the goal frame as described above. Figure out two pairs (r,s) and
; use the one which causes the smaller rotation.
LDF PXHOLD,AC0 ;Calculate Px↑2 + Py↑2
MULF AC0,AC0
LDF PYHOLD,AC1
MULF AC1,AC1
ADDF AC1,AC0 ;AC0 ← Px↑2 + Py↑2 = D↑2
STF AC0,AC2 ;Store D↑2 in AC2
LDF D3,AC1 ;Calculate D3↑2, store in AC1
MULF AC1,AC1
SUBF AC1,AC0 ;AC0 ← D↑2 - D3↑2
SQRFIX AC0 ;If it's negative but not too much, make it zero
CHECKF AC0 ;Is it negative?? If so, no sol'n!
BGE 10$ ; but if D↑2 ≥ D3↑2, it's OK
MOV #JT1,R3 ;Set bit for no sol'n, joint 1
JMP 100$ ; & exit
10$: GOSUB SQRTF ;Find sqrt (...), put in AC0
STF AC0,SQ ;Store sqrt (D↑2-D3↑2)
LDF PXHOLD,AC0 ;Now calculate r1,s1 and r2,s2
MULF D3,AC0 ;r1 ← (Px*D3 - Py*sq)/D↑2
LDF PYHOLD,AC1
MULF SQ,AC1
SUBF AC1,AC0
DIVF AC2,AC0 ; remember, D↑2 is in AC2
STF AC0,T1 ;Store 1st part (x component) of T1 (this is r1)
LDF PYHOLD,AC0 ;Calculate s1 = (Py*D3 + Px*sq)/D↑2
MULF D3,AC0
LDF PXHOLD,AC1
MULF SQ,AC1
ADDF AC1,AC0
DIVF AC2,AC0
STF AC0,T1+4 ; store in 2nd part (y component) of T1 (this is s1)
LDF PXHOLD,AC0 ;Calculate r2 = (Px*D3 + Py*sq)/D↑2
MULF D3,AC0
LDF PYHOLD,AC1
MULF SQ,AC1
ADDF AC1,AC0
DIVF AC2,AC0
STF AC0,T2 ; store 1st part (x component) of T2 (this is r1)
LDF PYHOLD,AC0 ;Calculate s2 = (Py*D3 - Px*sq)/D↑2
MULF D3,AC0
LDF PXHOLD,AC1
MULF SQ,AC1
SUBF AC1,AC0
DIVF AC2,AC0
STF AC0,T2+4 ; store 2nd part (y component) of T2 (this is s2)
MOV #T1,R4 ;Now find corresponding vector S to rotate the
MOV #SA,R5 ; goal frame about. Store the S corresponding to
GOSUB FINDS ; T1 in SA.
STF AC2,RANG1 ;Rotation angle needed for T1 - store in RANG1.
MOV #T2,R4 ;Same for vector SB corresponding to T2.
MOV #SB,R5
GOSUB FINDS
STF AC2,RANG2
COMPF RANG1,AC2 ;Is RotAng1 ≤ RotAng2? We want it this way.
BLE 30$ ;If SA means smaller rotation, use it & br
EXCHGF T1,T2 ;Switch things so SA is the smaller rotation.
EXCHGF T1+4,T2+4
EXCHGF RANG1,RANG2
EXCHGF SA,SB
EXCHGF SA+4,SB+4
EXCHGF SA+10,SB+10
30$: MOV #SA,R4 ;Now do the actual rotation. The vectors to be
LDF RANG1,AC2 ;rotated start at AXHOLD & OXHOLD. Rotate about SA
GOSUB ROT3 ;by RANG1 degrees.
GOSUB OFSETS ;Account for length of tool.
GOSUB GETSOL ;See if there is a solution.
TSTB R3 ;Was there a solution? If so, br
BEQ 100$
MOV #FARBIT,FAR ;Means we didn't find close sol'n
GOSUB SSETUP ;Set up arguments again (from R0)
MOV #SB,R4 ;Rotate about SB now
LDF RANG2,AC2
GOSUB ROT3
GOSUB OFSETS ;Again account for the length of the tool
GOSUB GETSOL ;Again try to get a solution.
100$: BIS FAR,R3 ;Set bit for far sol'n
MOV R3,R0 ;Set up result - bits for no joint sol'n
POP R4
POP R3
POP R2
RTS PC
; ["PUMSOL" - GETSOL]
GETSOL: MOV #1,CLOSE1 ;try to get "near" solutions for jts 1 & 2
MOV #1,CLOSE2
SOLV1: CLR R3 ;assume exact solution found
JSR PC,GET1 ;get solution for joint angle 1
STF AC0,@TH1(R1) ;and store in result joint angle vector
MOV #TH1,R5 ;set R5 to joint 1 for check.
JSR PC,CHKJNT ;see if within joint stops. If not, set error flag
TST R3 ;was joint 1 solution invalid?
BNE RETRY ;if not ok, try another solution
JSR PC,GET3 ;generate 2 possible angles for theta3.
JSR PC,GET2 ;solve for theta2, decide which theta3 to use too.
STF AC0,@TH2(R1) ;save result: theta2.
MOV #TH2,R5 ;check joint 2 now
JSR PC,CHKJNT ;see if within joint stops.
TST R3
BNE RETRY ;if jt2 sol'n invalid, retry the solution
LDF @TH3(R1),AC0 ;now we'll check our result for theta3
MOV #TH3,R5 ;about to check joint 3
JSR PC,CHKJNT ;see if within joint stops. if not, set error flag
TST R3 ;solution for joint 3 valid?
BNE RETRY ;if not, see if there are more sol'ns to generate.
JSR PC,GET4 ;get sol'ns for joints 4,5 and 6
STF AC0,@TH4(R1)
JSR PC,GET5
STF AC0,@TH5(R1)
JSR PC,GET6
STF AC0,@TH6(R1)
;If theta4=180, rotate the hand 180 degrees. Also check for degenerate solution.
JSR PC,FIX4
LDF @TH4(R1),AC0 ;put joint4 angle in AC0 for check
MOV #TH4,R5 ;about to check joint 4
JSR PC,CHKJNT ;is angle out of range? If so, set R3 to indicate.
TST R3 ;if invalid sol'n, then retry it
BNE RETRY
LDF @TH5(R1),AC0
MOV #TH5,R5
JSR PC,CHKJNT
TST R3 ;if invalid sol'n, then retry it
BNE RETRY
LDF @TH6(R1),AC0
MOV #TH6,R5
JSR PC,CHKJNT
TST R3 ;if invalid sol'n, then retry it
BEQ SEXIT ;but if sol'n is good, then exit.
RETRY: TST CLOSE2 ;if both close1 and close2 are 0, we give up
BEQ 15$ ;15$ means we keep checking.
CLR CLOSE2 ;try far solution for joint 2
BR 50$ ;go generate joint 1 solution
15$: TST CLOSE1 ;did we already try far solution for joint 1?
BEQ SEXIT ;if so, we give up. Exit.
MOV #1,CLOSE2 ;try far Jt1, near Jt2.
CLR CLOSE1 ;means generate far solution
50$: MOV #FARBIT,FAR ;Means this is not the close sol'n
BR SOLV1
SEXIT: RTS PC ;return to caller
; ["PUMSOL" - SSETUP]
; Routine to get the vector components out of the array pointed to by R0
; and put them into `hold' words so we can modify the goal frame if needed.
; Note we don't use the `N' vector in the calculation, so there's no
; need to retrieve it from the array.
; Due to the fact that the x-y axis of the Green PUMA's world is rotated
; 180 degrees from that used by AL, we need to change the N and O vectors
; accordingly. The appropriate transformation is to negate the X and Y
; components of these vectors.
; We also need to account for the fact that the origin of the PUMA's world is
; at the center of its shoulder, while the origin of AL's world is at the corner
; of the table. So we need to offset the position vectors by the appropriate
; amount. The transformation is: (x,y,z) → (x0-x,y0-y,z-z0) for the Green arm
; and (x,y,z) → (x-x0,y-y0,z-z0) for the Red arm, where (x0,y0,z0) is the
; location of the PUMA origin in AL's world and (x,y,z) is the goal point.
; This routine is entered with PHHICH pointing to a table of origin offsets
; for the arm in question. This table contains the X,Y and Z offsets of the
; arm origin, as three floating-pt numbers.
SSETUP: MOV PWHICH,R5 ;Set up pointer to arm constants
MOV OX(R0),OXHOLD ;COPY 1ST PART OF FLOATING-PT NUMBER
MOV OX+2(R0),OXHOLD+2 ;AND 2ND PART
MOV OY(R0),OYHOLD
MOV OY+2(R0),OYHOLD+2
MOV OZ(R0),OZHOLD
MOV OZ+2(R0),OZHOLD+2
MOV AX(R0),AXHOLD ;SAVE THE A VECTOR ALSO
MOV AX+2(R0),AXHOLD+2 ; AND NEGATE X AND Y COMPONENTS
MOV AY(R0),AYHOLD
MOV AY+2(R0),AYHOLD+2
MOV AZ(R0),AZHOLD
MOV AZ+2(R0),AZHOLD+2
CMP R5,#GCON ;IS IT THE GREEN ARM?
BNE 5$ ;IF NOT (RED ARM) DON'T ROTATE X-Y AXES.
NEGF OXHOLD ;NEGATE TO ACCOUNT FOR 180 DEG ROTATION
NEGF OYHOLD
NEGF AXHOLD
NEGF AYHOLD
LDF PX(R0),AC0 ;GET X COORDINATE OF POSITION VECTOR
NEGF AC0 ;CALCULATE -X + X0
ADDF XCOORD(R5),AC0 ;ADD X OFFSET
STF AC0,PXHOLD ;AND STORE IT
LDF PY(R0),AC0 ;SAME FOR Y COORD
NEGF AC0
ADDF YCOORD(R5),AC0
STF AC0,PYHOLD
BR 10$ ;CONTINUE AND FIND OFFSET Z COORD.
5$: LDF PX(R0),AC0 ;RED ARM: GET X-COORD OF POSITION VECTOR
SUBF XCOORD(R5),AC0 ;CALCULATE X-X0
STF AC0,PXHOLD ;AND SAVE OFFSET X.
LDF PY(R0),AC0 ;SAME FOR Y COORD
SUBF YCOORD(R5),AC0
STF AC0,PYHOLD
10$: LDF PZ(R0),AC0 ;AND Z COORD
SUBF ZCOORD(R5),AC0 ; (EXCEPT FOR Z IT'S Z - Z0)
STF AC0,PZHOLD
RTS PC
; ["PUMSOL" - ROTATION ROUTINES]
;ROT3 rotates both the O and A vectors about (R4) by an amount equal to
;theta = AC2 (and N is thus implicitly rotated too.)
ROT3: PUSH R5
MOV #OXHOLD,R5 ;Rotate O vector
GOSUB ROTATE
MOV #AXHOLD,R5 ;Rotate A
GOSUB ROTATE
POP R5
RTS PC
;ROTATE takes a vector S=(R4) and a vector V=(R5) and an angle theta=AC2.
;It rotates the vector V about the vector S by theta degrees, using Rodrigues'
;formula:
; V' = V cosα + SxV sinα + (V⊗S) (1-cosα) S
;where α is the rotation angle in AC2.
ROTATE: PUSHF AC0
PUSHF AC1
PUSHF AC2 ;Save AC2 since SNCOS garbages it.
PUSH R0
PUSH R1
PUSH R2
MOV R5,R0 ;Copy V into OLDV
MOV #OLDV,R1
GOSUB CPYVEC
MOV R4,R0 ;Get S x V, put into V (result is now forming)
MOV #OLDV,R1
MOV R5,R2
GOSUB XPROD
STF AC2,AC0 ;Put theta in AC0 for SNCOS call
GOSUB SNCOS ;Returns AC0 = sin(theta)
MOV R5,R0 ;Multiply V by sin(theta)
GOSUB SCAMUL
MOV #OLDV,R0 ;Move old V to temp
MOV #TEMPV,R1
GOSUB CPYVEC
STF AC1,AC0 ;Multiply temp (=V) by cos(theta)
MOV #TEMPV,R0
GOSUB SCAMUL
MOV #TEMPV,R0 ;Add temp (= V cos(theta)) to result (in V)
MOV R5,R1
GOSUB ADDVEC
MOV R4,R0 ;Copy S into temp
MOV #TEMPV,R1
GOSUB CPYVEC
MOV R4,R0 ;Find S dot V, put in AC0
MOV #OLDV,R1
GOSUB DOTPRO
NEGF AC1 ;AC1 ← -cos(theta)
ADDF FLT1,AC1 ;AC1 ← 1-cos(theta)
MULF AC1,AC0 ;AC0 ← (S⊗V) * (1-cos(theta))
MOV #TEMPV,R0 ;Multiply temp by above
GOSUB SCAMUL
MOV #TEMPV,R0 ;Add result of above to result
MOV R5,R1
GOSUB ADDVEC ;Result is now rotated V
POP R2
POP R1
POP R0
POPF AC2
POPF AC1
POPF AC0
RTS PC
; ["PUMSOL" - FINDS]
; Given a desired plane the hand must lie in, given by the vector (R4),
; this routine will calculate the vector about which to rotate the goal
; frame (we call it S) and return this vector in (R5). The necessary
; angle of rotation is returned in AC2.
FINDS: PUSHF AC0
PUSHF AC1
PUSH R0
PUSH R1
PUSH R2
LDF AXHOLD,AC0 ;Get X component of A
ABSF AC0 ; (use absolute value for comparison)
COMPF FLT001,AC0 ;Is it less than 0.001?
BLT 5$ ;If not, skip & continue
LDF AYHOLD,AC0 ;Get Y component of A
ABSF AC0 ; (use absolute value for comparison)
COMPF FLT001,AC0 ;Is it less than 0.001?
BGT 50$ ; if so, go do "no rotation" - degenerate
; Figure out which normal to the plane to use: T or T'.
5$: CLRF ZCOORD(R4) ;Make z component of (r,s,0) a zero to be sure.
LDF AXHOLD,AC0 ;Form Ax*Ax in AC0
MULF AC0,AC0
LDF AYHOLD,AC1 ;Put Ay*Ay in AC1
MULF AC1,AC1
ADDF AC1,AC0 ;AC0 ← Ax*Ax + Ay*Ay
GOSUB SQRTF ;Find norm of A's projection on X-Y plane, into AC0
LDF XCOORD(R4),AC1 ;AC1 ← r
MULF AXHOLD,AC1 ;AC1 ← Ax*r
NEGF AC1 ;AC1 ← -Ax*r
LDF YCOORD(R4),AC2 ;AC2 ← s
MULF AYHOLD,AC2 ; Ay*s
SUBF AC2,AC1 ;AC1 ← -Ax*r - Ay*s
DIVF AC0,AC1 ;Divide by norm of A
STF AC1,AC0 ;Put result in AC0 for ACOS
GOSUB ACOS ;AC0 ← acos((-r*ax-s*ay)/normA)
COMPF FLT90,AC0 ;Is 90 ≤ acos(...)? If so, skip & use T
BLE 20$ ;But if acos(...) < 90 then set T ← T'
NEGF XCOORD(R4) ; by negating r and s.
NEGF YCOORD(R4)
; Now we know which T we're going to use. Find vector S and rotation angle.
20$: MOV R4,R0 ;Copy T into J
MOV #HOLDJ,R1
GOSUB CPYVEC
MOV R4,R0 ;Find T⊗A, result ==> AC0
MOV #AXHOLD,R1
GOSUB DOTPRO
MOV #HOLDJ,R0 ;Multiply J by -T⊗A
NEGF AC0
GOSUB SCAMUL
MOV #AXHOLD,R0 ;Now add A to result. Result is then
MOV #HOLDJ,R1 ; J = A - (T⊗A)T
GOSUB ADDVEC
MOV #HOLDJ,R0 ;Make J a unit vector
GOSUB UNITIZ
MOV #HOLDJ,R0 ;Get ready to find theta. Find J ⊗ A, ==> AC0
MOV #AXHOLD,R1
GOSUB DOTPRO
GOSUB ACOS ;AC0 ← acos(J⊗A) = theta
STF AC0,AC2 ;Save rotation angle (to return)
MOV #AXHOLD,R0 ;Now find S = A x J. Store into (R5)
MOV #HOLDJ,R1
MOV R5,R2
GOSUB XPROD
;Now if the vector S is zero, don't do any rotations
MOV R5,R0 ;Get dot product of S with itself
MOV R5,R1 ; to form length↑2 of vector
GOSUB DOTPRO ;AC0 ← S⊗S
COMPF STHR,AC0 ;Is Threshold > length ?
BGT 50$ ;If so, go do "no rotation" case. Else continue
MOV R5,R0 ;Make S a unit vector
GOSUB UNITIZ
BR 100$ ;exit
50$: MOV #AXHOLD,R0 ;We have A almost vertical. So we don't rotate it.
MOV R5,R1 ;Just copy A into S
GOSUB CPYVEC
CLRF AC2 ;Angle to rotate = 0.
100$: POP R2
POP R1
POP R0
POPF AC1
POPF AC0
RTS PC
; ["PUMSOL" - GET1]
GET1: LDF PXHOLD,AC0 ;LOAD AC0 WITH PX
MULF AC0,AC0 ;AC0 ← PX*PX
LDF PYHOLD,AC1 ;AC1 ← PY
MULF AC1,AC1 ;AC1 ← PY*PY
ADDF AC1,AC0 ;AC0 ← PX↑2 + PY↑2
ABSF AC0 ;AC0 IS NOW R↑2
LDF D3,AC1 ;AC1 ← D3
MULF AC1,AC1 ;AC1 ← D3↑2
SUBF AC1,AC0 ;AC0 ← R↑2 - D3↑2
SQRFIX AC0 ;IF IT'S NEGATIVE BUT NOT TOO MUCH, MAKE IT ZERO.
CHECKF AC0 ;IF THIS IS NEGATIVE, WE'RE IN TROUBLE
BGE 1$ ;IF OK, CONTINUE
BIS #JT1,R3 ;ELSE SOLUTION IS INVALID FOR JOINT 1
RTS PC ;RETURN
1$: JSR PC,SQRTF ;FIND SQRT: AC0 ← SQRT(R↑2 + D3↑2)
STF AC0,TEMP1 ;STORE IN TEMP LOC. TEMP1←SQRT(...)
LDF PYHOLD,AC0 ;AC0 ← PY
LDF PXHOLD,AC1 ;AC1 ← PX
JSR PC,ATAN2 ;AC0 ← ARCTAN(PY,PX)
STF AC0,TRY1 ;TRY1 ← ARCTAN(PY,PX) (1ST PART OF RESULT)
STF AC0,TRY2 ;TRY2 ← ARCTAN(PY,PX) (1ST PART OF RESULT)
LDF TEMP1,AC1 ;AC1 ← SQRT(...) DENOMINATOR OF ARCTAN.
LDF D3,AC0 ;AC0 ← D3
JSR PC,ATAN2 ;FIND ARCTAN(D3, SQRT(...))
LDF TRY1,AC2 ;AC2 ← SOLUTION SO FAR
SUBF AC0,AC2 ;AC2 ← FINISHED SOL'N (LEFT-HANDED)
STF AC2,TRY1 ;TRY1 ← LEFT-HANDED SOL'N
LDF TEMP1,AC1 ;NOW CALCULATE RIGHT-HANDED SOLUTION
NEGF AC1 ; (THAT HAS NEGATIVE SQUARE ROOT)
LDF D3,AC0 ;RELOAD D3 INTO NUMERATOR FOR ARCTAN CALL
JSR PC,ATAN2 ;FIND ARCTAN (D3,-SQRT(...))
LDF TRY2,AC2 ;AC2 ← RIGHT-HANDED SOL'N SO FAR
SUBF AC0,AC2 ;AC2 ← FINISHED SOL'N (RIGHT-HANDED)
STF AC2,TRY2 ;TRY2 ← RIGHT-HANDED SOL'N
LDF TRY1,AC0 ;NOW SEE WHICH IS CLOSER TO CURRENT JT 1 ANGLE
LDF CUR1,AC1 ;PUT CURRENT JT 1 ANGLE IN AC1
JSR PC,DIST ;CALCULATE DIST, PUT IN AC2
STF AC2,AC3 ;AC3 ← DIST. BTW. TRY1 AND CURRENT
LDF TRY2,AC0 ;PUT SECOND SOLUTION IN AC0 FOR DIST. CALCULATION
JSR PC,DIST
COMPF AC3,AC2 ;SEE WHICH IS THE CLOSER SOLUTION
BLE 5$ ;IF TRY1 IS CLOSER THAN TRY2, BR
LDF TRY1,AC1 ;TRY1 WAS FARTHER THEN TRY2. EXCHANGE THEM
LDF TRY2,AC2
STF AC1,TRY2
STF AC2,TRY1
5$: LDF TRY1,AC0 ;SET RESULT IN AC0. ASSUME CLOSE SOLUTION
TST CLOSE1 ;THEN CHECK IF THIS IS RIGHT.
BNE 10$ ;IF OK, EXIT
LDF TRY2,AC0 ;WANT FAR SOLUTION, SO USE TRY2
10$: MOV #TH1,R5 ;WHICH JOINT TO NORMALIZE
JSR PC,NRMLIZ ;NORMALIZE ANGLE TO BE IN RANGE [0,360)
RTS PC
; ["PUMSOL" - GET3]
; THIS ROUTINE YIELDS TWO ANGLES FOR THETA3: TRY1 AND TRY2. EACH OF THESE
; ANGLES PRODUCES A DIFFERENT ANGLE FOR THETA2. WE PICK THE ANGLE FOR THETA2
; WHICH IS CLOSER (OR FARTHER) FROM THE CURRENT ANGLE, AND USING THIS CHOICE,
; SAVE THE CORRESPONDING ANGLE FOR THETA3.
GET3: LDF @TH1(R1),AC0 ;AC0 ← THETA1
JSR PC,SNCOS ;AC0 ← SIN(THETA1), AC1 ← COS(THETA1)
STF AC0,SIN1 ;STORE SIN(THETA1) ≡ SIN1
STF AC1,COS1 ;STORE COS(THETA1) ≡ C1
MULF PXHOLD,AC1 ;AC1 ← C1*PX
MULF PYHOLD,AC0 ;AC0 ← SIN1*PY
ADDF AC0,AC1 ;AC1 ← C1*PX + SIN1*PY = F11P
STF AC1,F11P ;SAVE F11P
MULF AC1,AC1 ;AC1 ← F11P↑2
LDF PZHOLD,AC0 ;AC0 ← PZ
MULF AC0,AC0 ;AC0 ← PZ↑2
ADDF AC0,AC1 ;WE'RE FORMING THE SUM F11P↑2 + F12P↑2
LDF D4,AC0 ;AC0 ← D4
MULF AC0,AC0 ;AC0 ← D4↑2
STF AC0,AC2 ;AC2 IS FORMING THE SUM A3↑2 + D4↑2
SUBF AC0,AC1 ;AC1 ← F11P↑2 + F12P↑2 - D4↑2
LDF A3,AC0 ;AC0 ← A3
MULF AC0,AC0 ;AC0 ← A3↑2
ADDF AC0,AC2 ;AC2 ← D4↑2 + A3↑2
SUBF AC0,AC1 ;AC1 ← ... - A3↑2
LDF A2,AC0 ;AC0 ← A2
MULF AC0,AC0 ;AC0 ← A2↑2
MULF AC0,AC2 ;AC2 ← A2↑2 * (D4↑2 + A3↑2)
SUBF AC0,AC1 ;AC1 ← F11P↑2 + F12P↑2 - D4↑2 - A3↑2 - A2↑2
STF AC1,TEMP2 ;SUM IS D IN THE WRITEUP -- SAVE D IN TEMP2
MULF FLT4,AC2 ;AC2 IS NOW E IN WRITEUP
MULF AC1,AC1 ;AC1 ← D↑2
SUBF AC1,AC2 ;AC2 ← E - D↑2
STF AC2,AC0 ;AC0 ← E-D↑2
SQRFIX AC2 ;IF AC2 IS NEGATIVE BUT NOT TOO MUCH, MAKE IT 0.
CHECKF AC2 ;SEE IF IT'S LESS THAN ZERO
BGE 5$ ;IF OK, BR
BIS #JT3,R3 ;OTHERWISE JOINT 3 SOL'N IS NO GOOD
RTS PC ;RETURN TO CALLER.
5$: JSR PC,SQRTF ;PUT SQRT(E-D↑2) BACK INTO AC0
STF AC0,TEMP1 ;TEMP1 ← SQRT(E-D↑2)
LDF A3,AC0 ;AC0 ← A3
LDF D4,AC1 ;AC1 ← D4
NEGF AC1 ;AC1 ← -D4
JSR PC,ATAN2 ;AC0 ← ARCTAN(A3, -D4)
STF AC0,TRY1 ;STORE ARCTAN AS 1ST PART OF RESULT
STF AC0,TRY2 ;STORE ARCTAN AS 1ST PART OF RESULT
LDF TEMP2,AC0 ;AC0 ← D (FROM TEMP2)
LDF TEMP1,AC1 ;AC1 ← SQRT(E-D↑2)
JSR PC,ATAN2 ;PUT ARCTAN(D,SQRT(...)) INTO AC0
NEGF AC0 ;AC0 ← -ATAN(...)
ADDF TRY1,AC0 ;AC0 ← THE "ABOVE" SOLUTION
MOV #TH3,R5 ;NORMALIZE JOINT 3 ANGLE
JSR PC,NRMLIZ ;PUT ANGLE IN RANGE
STF AC0,TRY1 ;STORE ABOVE SOL'N IN TRY1
LDF TEMP1,AC1 ;NOW DO THE "BELOW" SOLUTION, WHICH HAS
NEGF AC1 ;THE SQUARE ROOT NEGATED.
LDF TEMP2,AC0 ;LOAD NUMERATOR (D)
JSR PC,ATAN2 ;GET ARCTAN
NEGF AC0 ;AC0 ← -ATAN(...)
ADDF TRY2,AC0 ;AC0 ← THE COMPLETE "BELOW" SOLUTION.
MOV #TH3,R5
JSR PC,NRMLIZ ;NORMALIZE THE RESULT
STF AC0,TRY2 ;STORE BELOW SOL'N IN TRY2
RTS PC ;BACK TO CALLER
; ["PUMSOL" - GET2]
GET2: LDF TRY1,AC0 ;PUT 1ST TRY FOR THETA3 INTO AC0
JSR PC,CALC2 ;FIGURE OUT CORRESPONDING THETA2.
STF AC0,ANG2T1 ;STORE IN ANGLE2, TRY1
LDF TRY2,AC0 ;DO THE SAME FOR THE OTHER ANGLE
JSR PC,CALC2
STF AC0,ANG2T2 ;STORE 2ND TRY FOR ANGLE 2
LDF CUR2,AC1 ;AC1 ← CURRENT THETA2
JSR PC,DIST ;AC2 ← ANGLE BTW. TRY#2 AND CURRENT
STF AC2,AC3 ;AC3 ← 2ND DISTANCE
LDF ANG2T1,AC0 ;NOW FIND DISTANCE FOR 1ST TRY
JSR PC,DIST ;AC2 ← ANGLE BTW. TRY#1 AND CURRENT
COMPF AC2,AC3 ;COMP DISTANCE1 TO DISTANCE2
BLE 5$ ;IF 1ST IS NOT GREATER THAN 2ND, OK & SKIP
LDF TRY1,AC1 ;2ND WAS CLOSER THAN 1ST. EXCHANGE TRY1 AND TRY2.
LDF TRY2,AC2
STF AC1,TRY2
STF AC2,TRY1
LDF ANG2T1,AC1 ;ALSO EXCHANGE TRIES FOR ANGLE 2.
LDF ANG2T2,AC2
STF AC1,ANG2T2
STF AC2,ANG2T1
5$: LDF TRY1,AC0 ;ASSUME WE WANT THE CLOSE ANGLE
STF AC0,@TH3(R1) ;SO STORE IT FIRST
LDF ANG2T1,AC0 ;PUT ANGLE2 IN AC0
TST CLOSE2 ;DID WE REALLY WANT THE CLOSE ONE FOR ANGLE 2?
BNE 10$ ;IF SO, EXIT
LDF TRY2,AC1 ;WANTED FAR ANGLE, SO USE IT
STF AC1,@TH3(R1)
LDF ANG2T2,AC0
10$: RTS PC ;TO CALLER
; WE ASSUME THETA3 IS IN AC0, AND UPON EXIT, AC0 CONTAINS THETA2.
CALC2: STF AC0,@TH3(R1) ;TEMPORARILY SAVE ATTEMPT AT ANGLE 3.
JSR PC,SNCOS ;AC0 ← SIN(THETA3) AC1 ← COS(THETA3)
STF AC0,SIN3 ;SAVE THE SINE AND COSINE
STF AC1,C3
MULF A2,AC1 ;AC1 ← A2*C3
ADDF A3,AC1 ;AC1 ← A2*C3 + A3 ≡ W1 (W1 WILL STAY IN AC1 A WHILE)
MULF A2,AC0 ;AC0 ← A2*S3
ADDF D4,AC0 ;AC0 ← D4 + A2*S3 ≡ W2
STF AC0,AC2 ;AC2 ← W2 (W2 WILL STAY IN AC2 FOR A WHILE)
LDF F11P,AC0 ;AC0 ← F11P
MULF AC2,AC0 ;AC0 ← W1*F11P
LDF PZHOLD,AC3 ;AC3 ← PZ
MULF AC1,AC3 ;AC3 ← W1*PZ
SUBF AC3,AC0 ;AC0 ← W2*F11P - W1*PZ
MULF F11P,AC1 ;AC1 ← W1*F11P
MULF PZHOLD,AC2 ;AC2 ← W2*PZ
ADDF AC2,AC1 ;AC1 ← W1*F11P + W2*PZ
JSR PC,ATAN2 ;AC0 ← ARCTAN(W2*F11P-W1*PZ, W1*F11P+W2*PZ)
SUBF @TH3(R1),AC0 ;AC0 ← THETA23 - THETA3 = THETA2
MOV #TH2,R5 ;NORMALIZE JOINT 2 ANGLE
JSR PC,NRMLIZ ;NORMALIZE THE RESULT (THETA2)
RTS PC ;TO CALLER
; ["PUMSOL" - GET4]
GET4: LDF @TH2(R1),AC0 ;AC0 ← THETA2
ADDF @TH3(R1),AC0 ;AC0 ← THETA2 + THETA3
JSR PC,SNCOS ;AC0 ← SIN(T2+T3) AC1 ← COS(T2+T3)
STF AC0,S23 ;STORE SINE
STF AC1,C23 ;AND COSINE
LDF COS1,AC1 ;AC1 ← C1
MULF AYHOLD,AC1 ;AC1 ← C1*AY
LDF SIN1,AC2 ;AC2 ← SIN1
MULF AXHOLD,AC2 ;AC2 ← SIN1*AX
SUBF AC2,AC1 ;AC1 ← -SIN1*AX + C1*AY ≡ F13A
STF AC1,F13A ;STORE RESULT -- THIS IS F13A
LDF AXHOLD,AC2 ;AC2 ← AX
MULF COS1,AC2 ;AC2 ← C1*AX
LDF SIN1,AC1 ;AC1 ← SIN1
MULF AYHOLD,AC1 ;AC1 ← SIN1*AY
ADDF AC1,AC2 ;AC2 ← C1*AX + SIN1*AY ≡ F11A
STF AC2,F11A ;STORE THIS RESULT FOR LATER USE
LDF C23,AC1 ;AC1 ← C23
MULF AC1,AC2 ;AC2 ← C23*F11A
LDF AZHOLD,AC1 ;AC1 ← AZ
MULF S23,AC1 ;AC1 ← S23*AZ
STF AC1,S23AZ ;STORE INTERMEDIATE RESULT
SUBF AC1,AC2 ;AC2 ← C23*F11A - S23*AZ
STF AC2,T4BOT ;KEEP THIS INTERMEDIATE FOR USE IN GET5.
LDF F13A,AC0 ;GET READY FOR ARCTAN: AC0 ← F13A
STF AC2,AC1 ;AC1 ← BOTTOM OF ARCTAN EXPR
JSR PC,ATAN2 ;PUT ARCTAN (F13A,T4BOT) INTO AC0
MOV #TH4,R5 ;NORMALIZE ANGLE 4
JSR PC,NRMLIZ ;RESULT (AC0) IS THETA4 -- NORMALIZE IT
RTS PC
; ["PUMSOL" - GET5]
GET5: LDF @TH4(R1),AC0 ;GET READY TO FIND SIN AND COS OF THETA4
JSR PC,SNCOS ;AC0 ← SIN(THETA4) AC1 ← COS(THETA4)
STF AC0,SIN4 ;STORE SIN AND COS OF THETA4
STF AC1,C4
MULF F13A,AC0 ;AC0 ← S4*F13A
MULF T4BOT,AC1 ;AC1 ← C4*T4BOT
ADDF AC1,AC0 ;AC0 ← C4*T4BOT + S4*F13A
LDF S23,AC1 ;AC1 ← S23
MULF F11A,AC1 ;AC1 ← S23*F11A
LDF C23,AC2 ;AC2 ← C23
MULF AZHOLD,AC2 ;AC2 ← C23*AZ
ADDF AC2,AC1 ;AC1 ← S23*F11A + C23*AZ
JSR PC,ATAN2 ;AC0 ← ARCTAN(...,...) ≡ THETA5
MOV #TH5,R5 ;TO NORMALIZE ANGLE 5
JSR PC,NRMLIZ ;NORMALIZE THE RESULT
COMPF FLT180,AC0 ;COMPARE 180 TO THETA5. IF THETA5>180,WE'RE NOT DONE
;(NOTE IF α>180, IN THE RANGE (-180,180), α<0)
BGE 10$ ;IF 180 ≥ THETA5, OK, SO EXIT
STF AC0,AC3 ;STORE THETA5 AWAY SO WE CAN RESTORE IT LATER.
LDF @TH4(R1),AC0 ;IF THETA5>180, ADD 180 TO THETA4
ADDF FLT180,AC0 ;AC0 ← THETA4 + 180
MOV #TH4,R5
JSR PC,NRMLIZ ;NORMALIZE NEW THETA4
STF AC0,@TH4(R1) ;SAVE NEW THETA4
NEGF SIN4 ;WE MUST NEGATE OLD S4 AND C4
NEGF C4 ; SINCE SIN(α+180)=-SIN(α) AND SAME FOR COS
STF AC3,AC0 ;PUT THETA5 BACK INTO AC0
10$: RTS PC ;RETURN TO CALLER
; ["PUMSOL" - GET6]
GET6: LDF @TH5(R1),AC0 ;AC0 ← THETA5
JSR PC,SNCOS ;AC0 ← SIN(THETA5) AC1 ← COS(THETA5)
STF AC0,SIN5 ;STORE SIN(THETA5)
STF AC1,C5 ;STORE COS(THETA5)
LDF SIN1,AC0 ;AC0 ← SIN1
MULF OYHOLD,AC0 ;AC0 ← SIN1*OY
LDF COS1,AC1 ;AC1 ← C1
MULF OXHOLD,AC1 ;AC1 ← C1*OX
ADDF AC1,AC0 ;AC0 ← C1*OX + SIN1*OY ≡ F11O
STF AC0,F11O ;STORE F11O
LDF SIN1,AC0 ;AC0 ← SIN1
MULF OXHOLD,AC0 ;AC0 ← SIN1*OX
LDF COS1,AC1 ;AC1 ← C1
MULF OYHOLD,AC1 ;AC1 ← C1*OY
SUBF AC0,AC1 ;AC1 ← C1*OY - SIN1*OX
STF AC1,F13O ;THIS IS F13O, STORE IT.
LDF C23,AC0 ;AC0 ← C23
MULF F11O,AC0 ;AC0 ← C23*F11O
LDF S23,AC1 ;AC1 ← S23
MULF OZHOLD,AC1 ;AC1 ← S23*OZ
SUBF AC1,AC0 ;AC0 ← C23*F11O - S23*OZ
STF AC0,PART1 ;THIS IS A MAJOR PART OF THE RESULT, SAVE IT.
MULF C4,AC0 ;AC0 ← C4*PART1
LDF SIN4,AC1 ;AC1 ← S4
MULF F13O,AC1 ;AC1 ← S4*F13O
ADDF AC1,AC0 ;AC0 ← C4*PART1 + S4*F13O
MULF C5,AC0 ;AC0 ← C5*(.... + ....)
LDF S23,AC1 ;AC1 ← S23
MULF F11O,AC1 ;AC1 ← S23*F11O
LDF C23,AC2 ;AC2 ← C23
MULF OZHOLD,AC2 ;AC2 ← C23*OZ
ADDF AC2,AC1 ;AC1 ← S23*F11O + C23*OZ
MULF SIN5,AC1 ;AC1 ← S5*(.... + ....)
SUBF AC0,AC1 ;AC1 ← TOP PART OF ARCTAN
STF AC1,AC0 ;KEEP TOP PART IN AC0
LDF PART1,AC1 ;AC1 ← PART1
MULF SIN4,AC1 ;AC1 ← S4*PART1
LDF C4,AC2 ;AC2 ← C4
MULF F13O,AC2 ;AC2 ← C4*F13O
SUBF AC1,AC2 ;AC2 ← C4*F13O - S4*PART1
STF AC2,AC1 ;THIS IS DENOMINATOR FOR ARCTAN -- KEEP IN AC1
JSR PC,ATAN2 ;AC0 ← ARCTAN (TOP,BOTTOM)
MOV #TH6,R5 ;TO NORMALIZE ANGLE 6
JSR PC,NRMLIZ ;NORMALIZE RESULT (THIS IS THETA6)
RTS PC ;TO CALLER
; ["PUMSOL" - FIX4]
; This routine checks for:
; 1. |theta4| = 180 degrees. If this is the case, we make theta4 = 0 and
; negate theta5 and rotate theta6 accordingly.
; 2. |theta5| = 0. If this happens, it is a degenerate solution and
; we add theta4 to theta6 and make theta4 = 0.
FIX4: LDF @TH4(R1),AC0 ;AC0 ← THETA4. IF THIS IS 180, WE'RE NOT DONE YET.
ABSF AC0 ;MAY BE -180 TOO.
COMPF FLT179,AC0 ;ACCOUNT FOR ROUNDING ERRORS, MAY NOT = 180
BGE 100$ ;IF 179 ≥ |THETA4|, DO NOTHING.
CLRF @TH4(R1) ;SET THETA4 = 0
NEGF @TH5(R1) ;NEGATE THETA5
LDF @TH6(R1),AC0 ;AC0 ← THETA6
CHECKF AC0 ;PUT THETA6 INTO OPPOSITE SIDE: IF <0, ADD 180
BGT 10$
ADDF FLT180,AC0 ;ADD 180 TO THETA6
BR 12$ ;AND GO SAVE NEW THETA6
10$: SUBF FLT180,AC0 ;SUB 180 FROM THETA6
12$: MOV #TH6,R5 ;NORMALIZE THETA6
JSR PC,NRMLIZ
STF AC0,@TH6(R1) ;SAVE NEW THETA6
100$: LDF @TH5(R1),AC0 ;AC0 ← THETA5.
ABSF AC0 ;USE |THETA5|
COMPF FLT01,AC0 ;COMPARE TO 0.01 (ACCOUNT FOR ROUNDING ERRORS)
BLT 200$ ;IF 0.01 < THETA5, EXIT. BUT IF NOT, THEN
LDF @TH6(R1),AC0 ;AC0 ← THETA6
ADDF @TH4(R1),AC0 ;AND ADD THETA4
MOV #TH6,R5 ;NORMALIZE NEW THETA6 = THETA6 + THETA4
JSR PC,NRMLIZ
STF AC0,@TH6(R1) ;STORE BACK
CLRF @TH4(R1) ;PUT ZERO INTO THETA4
200$: RTS PC ;TO CALLER
; ["PUMSOL" - OFSETS]
; A routine to account for the length of the tool -- we offset the solution
; backwards along the tool Z axis (the A vector) by an amount equal to TOOLEN.
OFSETS: LDF AXHOLD,AC0 ;AC0 ← AX
MULF TOOLEN,AC0 ;AC0 ← AX*TOOLLENGTH
LDF PXHOLD,AC1 ;AC1 ← CURRENT PX
SUBF AC0,AC1 ;AC1 ← PX - AX*TOOLLENGTH
STF AC1,PXHOLD ;STORE OFFSET PX
LDF AYHOLD,AC0 ;AC0 ← AY
MULF TOOLEN,AC0 ;AC0 ← AY*TOOLLENGTH
LDF PYHOLD,AC1 ;AC1 ← PY
SUBF AC0,AC1 ;AC1 ← PY - AY*TOOLLENGTH
STF AC1,PYHOLD ;THIS IS OFFSET PY
LDF AZHOLD,AC0 ;AC0 ← AZ
MULF TOOLEN,AC0 ;AC0 ← AZ*TOOLLENGTH
LDF PZHOLD,AC1 ;AC1 ← PZ
SUBF AC0,AC1 ;AC1 ← PZ + AZ*TOOLLENGTH
STF AC1,PZHOLD ;THIS IS OFFSET PZ
RTS PC
; ["PUMSOL" - ANGLE MANIPULATION ROUTINES]
; NRMLIZ normalizes the angle in AC0 - it puts it in the correct range for
; that joint. The lower bound for the range (which is [lobound, lobound+360])
; is found in the table LOLIM.
; Call this routine with angle in AC0, joint offset in R5 (e.g. MOV #TH1,R5)
; R5 is altered!
; Returns with normalized angle in AC0.
NRMLIZ: PUSH R4
PUSHF AC1
ASL R5 ;MULT BY 2 TO GET OFFSET INTO 2-WORD LENGTH TABLES
LDF LOLIM(R5),AC1 ;GET LOW LIMIT OF RANGE FOR ANGLE
3$: COMPF AC0,AC1 ;IS ANGLE ≥ LOWLIMIT? IF SO, DON'T CHANGE IT
BGE 5$
ADDF FLT360,AC0 ;ADD 360 TO ANGLE
BR 3$ ;MAY STILL BE OUT OF RANGE -- KEEP TESTING.
5$: ADDF FLT360,AC1 ;PUT UPPER BOUND INTO AC1
7$: COMPF AC0,AC1 ;SEE IF IT'S BIGGER THAN RANGE
BLT 10$ ;IF 360 < ANGLE, EXIT
SUBF FLT360,AC0 ;WAS TOO BIG, SUBTRACT 360
BR 7$ ;IT MAY STILL BE TOO BIG, SO KEEP TESTING.
10$: POPF AC1
POP R4
RTS PC
; CHKJNT SEES IF A JOINT IS WITHIN ITS STOP LIMITS. IF NOT, THE APPROPRIATE
; BIT IN R3 IS SET TO INDICATE THAT IT IS OUT OF RANGE. CALLED WITH JOINT
; OFFSET IN R5 (E.G. MOV #TH1,R5) AND ANGLE IN AC0.
; R5 IS ALTERED!
CHKJNT: ASL R5 ;MULT BY 2 FOR OFFSET INTO FLTING-PT TABLE
COMPF LOSTOP(R5),AC0 ;IS LOW STOP > ANGLE?
BGT 5$ ;IF SO, ANGLE IS OUT OF BOUNDS - SET FLAG
COMPF HISTOP(R5),AC0 ;IS HIGH STOP < ANGLE?
BGE 10$ ;IF NOT, WE'RE OK.
5$: ASH #-2,R5 ;DIVIDE R5 BY 4 (GIVES 0,1,2,3,4)
PUSH R4 ;SAVE OLD R4
MOV #1,R4 ;SET TO 1 TO INDICATE JOINT 1 NO GOOD
ASH R5,R4 ;SHIFT LEFT BY JT PLACES. YIELDS JT# WHICH IS BAD
BIS R4,R3 ;SET APPROPRIATE BIT IN R3, THE ERROR FLAG.
POP R4
10$: RTS PC ;TO CALLER
; DIST CALCULATES THE DISTANCE IN DEGREES BETWEEN THE ANGLES IN AC0 AND AC1
; AND PUTS THE RESULT INTO AC2.
DIST: STF AC0,AC2 ;AC2 ← ANGLE1
SUBF AC1,AC2 ;FORM THE DIFFERENCE
ABSF AC2 ;TAKE ABSOLUTE VALUE OF DIFF.
COMPF FLT360,AC2 ;MAKE SURE DIFF IS < 360. IS 360 > DIFF?
BGT 3$ ;IF SO, CONTINUE -- WE'RE OK
SUBF FLT360,AC2
3$: COMPF FLT180,AC2 ;IS 180 ≥ DIFF?
BGE 5$ ;IF SO, EXIT
NEGF AC2 ;MAKE AC2 INTO 360-AC2.
ADDF FLT360,AC2
5$: RTS PC
; ["PUMSOL" - VECTOR MANIPULATION ROUTINES]
; In the following, all vectors contain 3 components. A vector which is
; pointed to by a register, say R0, is referred to by "(R0)".
;ADDVEC adds the vector (R0) to (R1).
ADDVEC: PUSHF AC0
PUSH R5
MOV #3,R5 ;Vector has 3 components
1$: LDF (R1),AC0 ;Get component of 2nd vector
ADDF (R0)+,AC0 ;Add component of 1st to it
STF AC0,(R1)+ ; and store back
SOB R5,1$
POP R5
POPF AC0
RTS PC
;DOTPRO finds the dot product of (R0) and (R1) and returns it in AC0.
DOTPRO: PUSHF AC1
PUSH R5
MOV #3,R5
CLRF AC0 ;Clear the result
1$: LDF (R0)+,AC1 ;Get component of 2nd vector
MULF (R1)+,AC1 ;and multiply by component of 1st.
ADDF AC1,AC0 ;Add to sum
SOB R5,1$
POP R5
POPF AC1
RTS PC
;CPYVEC copies the vector (R0) into (R1).
CPYVEC: PUSH R5
MOV #3,R5
1$: MOV (R0)+,(R1)+ ;Get 1st part of floating-pt number & copy
MOV (R0)+,(R1)+ ;copy 2nd half of floating-pt number
SOB R5,1$
POP R5
RTS PC
;SCAMUL performs a scalar multiply: multiplies the vector (R0) by AC0.
SCAMUL: PUSHF AC1
PUSH R5
MOV #3,R5
1$: LDF (R0),AC1 ;Get component
MULF AC0,AC1 ;and multiply by the scalar
STF AC1,(R0)+ ;Store back
SOB R5,1$
POP R5
POPF AC1
RTS PC
;XPROD calculates the cross product of (R0) and (R1) and puts it into (R2).
XPROD: PUSHF AC0
PUSHF AC1
LDF YCOORD(R0),AC0 ;AC0 ← Y0
MULF ZCOORD(R1),AC0 ;AC0 ← Y0*Z1
LDF YCOORD(R1),AC1 ;AC1 ← Y1
MULF ZCOORD(R0),AC1 ;AC1 ← Y1*Z0
SUBF AC1,AC0 ;AC0 ← Y0*Z1 - Y1*Z0
STF AC0,XCOORD(R2) ;Store X component of (R0)x(R1)
LDF XCOORD(R0),AC0 ;Similarly for Y component:
MULF ZCOORD(R1),AC0
LDF XCOORD(R1),AC1
MULF ZCOORD(R0),AC1
SUBF AC0,AC1
STF AC1,YCOORD(R2) ;Y component = X1*Z0 - X0*Z1
LDF XCOORD(R0),AC0 ;Now do the Z component
MULF YCOORD(R1),AC0
LDF XCOORD(R1),AC1
MULF YCOORD(R0),AC1
SUBF AC1,AC0
STF AC0,ZCOORD(R2) ;Z component = X0*Y1 - X1*Y0
POPF AC1
POPF AC0
RTS PC
;UNITIZ will unitize the vector (R0), i.e. make it into a unit vector.
UNITIZ: PUSHF AC0
PUSHF AC1
PUSH R5
MOV R0,R1 ;Use DOTPRO to get (x,y,z)⊗(x,y,z) = x*x + y*y + z*z
GOSUB DOTPRO ;AC0 ← x*x + y*y + z*z
SUB #<3*4>,R0 ;Restore R0 to whatever it used to be.
GOSUB SQRTF ;AC0 ← SQRT(X*X + Y*Y + Z*Z) = norm of vector
MOV #3,R5 ;Now go thru & divide (R0) by AC0.
1$: LDF (R0),AC1 ;Put component into AC1
DIVF AC0,AC1 ;Divide by norm
STF AC1,(R0)+ ;and store back
SOB R5,1$
POP R5
POPF AC1
POPF AC0
RTS PC
; ["PUMSOL" - LOCAL DATA]
DATA
;MISC. HOLDING AREAS AND TEMP.VARIABLES:
TRY1: .BLKW 2
TRY2: .BLKW 2
TEMP1: .BLKW 2
TEMP2: .BLKW 2
F11P: .BLKW 2
S23AZ: .BLKW 2
T4BOT: .BLKW 2
F11A: .BLKW 2
F13A: .BLKW 2
F11O: .BLKW 2
F13O: .BLKW 2
CUR1: .BLKW 2 ;CURRENT JOINT 1 ANGLE
CUR2: .BLKW 2
OXHOLD: .BLKW 2 ;FOR HOLDING THE COMPONENTS OF THE VECTORS
OYHOLD: .BLKW 2 ; IN THE GOAL FRAME.
OZHOLD: .BLKW 2
AXHOLD: .BLKW 2
AYHOLD: .BLKW 2
AZHOLD: .BLKW 2
PXHOLD: .BLKW 2
PYHOLD: .BLKW 2
PZHOLD: .BLKW 2
OLDV: .BLKW <3*2> ;TEMPORARY STORAGE FOR ROTATION ROUTINES
TEMPV: .BLKW <3*2>
HOLDJ: .BLKW <3*2>
SQ: .BLKW 2
T1: .BLKW <3*2>
T2: .BLKW <3*2>
SA: .BLKW <3*2>
SB: .BLKW <3*2>
RANG1: .BLKW 2
RANG2: .BLKW 2
PWHICH: .WORD 0 ;POINTER TO ARM CONSTANTS
CLOSE1: .WORD 0 ;1=GET CLOSE SOL'N FOR JOINT 1, 0=GET FAR SOL'N
CLOSE2: .WORD 0 ;SAME, FOR JOINT 2.
FAR: .WORD 0 ;0=GOT CLOSE SOL'N, ≠0=GOT A FAR SOL'N
STHR: .FLT2 1.0E-6 ;Length↑2 of S must be < this to be called zero.
CODE
;UPDATE - COMPUTES THE ARM TRANSFORM GIVEN THE JOINT ANGLES
;GIVEN THE JOINT ANGLES, THE RESULTING HAND POSITION AND ORIENTATION ARE
;DETERMINED AND STORED IN A GIVEN TRANSFORM "T". THE TRANSFORM IS A 4 BY 4
;MATRIX STORED BY COLUMNS WITH EACH VALUE REPRESENTED IN TABLE COORDINATES.
;NO INITIALIZATION NEEDS TO BE DONE ON THE T SINCE ALL THE ELEMENTS WILL BE
;UPDATED, EVEN THOUGH THE LAST ROW IS ALWAYS 0,0,0,1 . A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; MOV #T,R0 ;LOAD TRANSFORM ADDRESS IN R0
; MOV #THETA,R1 ;PTR TO A TABLE CONTAINING POINTERS TO THE
; ; JOINT ANGLES FOR ALL EXISTING ARM JOINTS,
; ; I.E., THE TABLE MUST HAVE 28 ELEMENTS.
; MOV #MECHSM,R2 ;CONSTANT INDICATING WHICH ARM TO USE
; JSR PC,UPDATE ;CALLED USING PC
;
;THE TRANSFORM WILL BE OF THE FOLLOWING FORM:
;
; | T1 T5 T9 T13 |
; | T2 T6 T10 T14 |
; | T3 T7 T11 T15 |
; | 0 0 0 1.0 |
;
;THE PROCEDURE CONSISTS OF GENERATION OF TWO MATRICES T1 AND T2, THE TRANSFORM
;FROM SHOULDER TO WRIST AND FROM WRIST TO HAND RESPECTIVELY, THEN MULTIPLYING
;THE TWO TO GIVE THE DESIRED TRANSFORM.
;
;THIS PROCEDURE MODIFIED ON JUNE 9TH, 1976 TO THE EXTENT THAT THE FIRST
;COL OF T2 IS NOT COMPUTED, AND THUS ONLY THE THIRD THREE COLS OF
;T ARE COMPUTED. THE FIRST COLUMN IS THEN FOUND BY TAKING THE CROSS PRODUCT
;OF THE SECOND AND THIRD COLUMNS.
;
;IF THE BLUE OR YELLOW HAND IS SPECIFIED AS THE MECHANISM, THE HAND OPENING
;POINTED TO BY THE "THETA" ARRAY IS RETURNED IN THE LOCATION POINTED TO BY
;"T". ALL NUMBERS SHOULD BE IN SINGLE PRECISION FLOATING POINT
;REPRESENATATION.
;EXECUTION TIME: 2080 microsecs
;REGISTERS USED:
;
; R0 - T TRANSFORM/LINK NUMBER
; R1 - THETA LIST/ARM CONSTANTS LIST POINTER
; R2 - BLUE/YELLOW ARM FLAG
; AC0,AC1,AC2,AC3,AC4,AC5 GARBAGED
; R0,R1 GARBAGED
;DEFINITIONS:
SN==%0
CS==%1
SN1==%4
CS1==%5
SN2==%0
CS2==%1
SN4==%3
CS4==%4
CS5==%5
; ["UPDATE"- HAND OR T6]
UPDATE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
BIT #YELARM+BLUARM+GRNARM+REDARM,R2 ;CHECK IT'S ONE OF THE ARMS
BNE 1$ ;IF IT'S AN ARM, BR
;SECTION TO RETURN HAND OPENINGS
;WHAT IF IT'S A PUMA???
ADD #12.,R1 ;GET POINTER TO YELLOW HAND OPENING
BIT #YELHND,R2 ;CHECK WHICH HAND WE ARE DEALING WITH
BNE .+6 ;SKIP IF ITS THE YELLOW HAND MECH. BIT
ADD #14.,R1 ;ELSE GET THE BLUE HAND POINTER
LDF @(R1),AC0 ;GET THE PREVIOUS HAND OPENING
STF AC0,(R0) ;RETURN THE CORRECT HAND OPENING AND EXIT
BR 3$
;ARM HANDLER
1$: BIT #GRNARM+REDARM,R2 ;IS IT A PUMA??
BEQ 10$ ;IF NOT, GO INTO STANFORD ARM SOL'N
MOV #GCON,R5 ;ASSUME GREEN ARM & SET R5 TO CONSTANT TABLE
ADD #32.,R1 ; POINT R1 TO GREEN ANGLES
BIT #REDARM,R2 ;WAS IT REALLY THE RED ARM??
BEQ 20$ ;IF NOT, BR
MOV #RCON,R5 ;SET R5 TO RED ARM CONSTANTS
ADD #14.,R1 ;POINT TO RED ARM ANGLES.
20$: JSR PC,PUMUPD ;UPDATE PUMA MATRIX
BR 3$ ;RETURN
10$: MOV #YCON,R3 ;ADDR OF YELLOW ARM PHYSICAL CONSTANTS
BIT #YELARM,R2 ;CHECK IF YELLOW OR BLUE ARM
BNE 2$ ;SKIP IF YELLOW ARM
MOV #BCON,R3 ;ELSE GET ADDR OF BLUE ARM CONSTANTS
ADD #14.,R1 ;POINT TO BLUE ARM JOINT ANGLES
2$: CLR R4 ;COMPUTE T0_3 & T3_6: ADD IN BASEX,BASEY
JSR PC,T0336U
MOV #T311,R1 ;RESULT ← T3*T6
MOV #T011,R2
JSR PC,MATMUL
;RESTORE REGISTERS AND EXIT
3$: MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;END OF "UPDATE"
; ---> PUMUPD < --- THE PUMA UPDATE ROUTINE
; "PUMUPD" is a routine which, given a vector of six joint angles for the PUMA,
; will return a 4 x 4 matrix which contains the position and orientation of
; the hand.
; ARGUMENTS:
; R0 - Points to a matrix which will contain, upon exit, the Cartesian
; coordinates and orientation (Euler angles) of the manipulator.
; This will be stored in column-major order, the same as the matrix
; described in "solve".
; R1 - Points to a vector, 6 words long, each word of which points to
; a single-precision floating-point number which is an angle in
; degrees. The 6 angles thus referenced are assumed to be the
; joint angles of the arm.
; R5 - Points to the arm constants for this manipulator, i.e. either
; RCON or GCON. These are used to get the offset of the base of
; the arm in AL's world.
; REGISTERS USED:
; R0,R1,R5 For passing arguments. They are not changed.
; AC0,AC1,AC2: Destroyed.
; [CONTINUATION OF PUMUPD: THE MAIN PART]
PUMUPD: MOV R5,PWHICH ;SAVE ADDR OF CONSTANTS TABLE FOR THIS ARM
JSR PC,DOTRIG ;FIGURE OUT SINES & COSINES NEEDED
JSR PC,GETN ;CALCULATE & STORE N VECTOR
JSR PC,GETO ;STORE O (ORIENTATION) VECTOR
JSR PC,GETA ;STORE THE A (APPROACH) VECTOR
JSR PC,GETP ;LAST, THE P (POSITION) VECTOR
JSR PC,OFSETW ;OFFSET RESULT TO ACCT. FOR LENGTH OF TOOL.
;ALSO ACCOUNT FOR ROTATION & OFFSET OF ORIGIN
RTS PC ;TO CALLER
; [CONTINUATION OF PUMUPD: DOTRIG]
; Retreive the joint angles from the vector pointed to by R1, and calculate
; sine & cosine for them all.
DOTRIG: LDF @TH1(R1),AC0 ;AC0 ← THETA1
JSR PC,SNCOS ;AC0 ← SIN(THETA1), AC1 ← COS(THETA1)
STF AC0,SIN1 ;STORE SIN1=SIN(THETA1)
STF AC1,COS1 ;STORE C1=COS(THETA1)
LDF @TH2(R1),AC0 ;DO THE SAME FOR THE OTHER ANGLES
JSR PC,SNCOS
STF AC0,SIN2
STF AC1,COS2
LDF @TH4(R1),AC0
JSR PC,SNCOS
STF AC0,SIN4
STF AC1,C4
LDF @TH5(R1),AC0
JSR PC,SNCOS
STF AC0,SIN5
STF AC1,C5
LDF @TH6(R1),AC0
JSR PC,SNCOS
STF AC0,SIN6
STF AC1,C6
LDF @TH2(R1),AC0 ;AC0 ← THETA2
LDF @TH3(R1),AC1 ;AC1 ← THETA3
ADDF AC1,AC0 ;AC0 ← THETA2+THETA3
JSR PC,SNCOS ;FIGURE OUT SIN & COS OF (THETA2 + THETA3)
STF AC0,S23 ;S23 = SIN(THETA2+THETA3)
STF AC1,C23
RTS PC ;THAT'S IT
; [CONTINUATION OF PUMUPD: GETN]
; Calculate the N vector of the Cartesian solution.
GETN: LDF C5,AC0 ;AC0 ← C5
MULF C6,AC0 ;AC0 ← C5*C6
STF AC0,AC1 ;AC1 ← C5*C6
MULF C4,AC0 ;AC0 ← C4*C5*C6
STF AC0,C4C5C6 ;STORE IT
MULF SIN4,AC1 ;AC1 ← S4*C5*C6
STF AC1,S4C5C6 ;STORE IT
LDF SIN4,AC0 ;AC0 ← S4
MULF SIN6,AC0 ;AC0 ← S4*SIN6
STF AC0,S4S6
LDF SIN5,AC0 ;AC0 ← S5
MULF C6,AC0 ;AC0 ← S5*C6
STF AC0,S5C6 ;STORE IT
LDF C4,AC0 ;AC0 ← C4
MULF SIN6,AC0 ;AC0 ← C4*SIN6
STF AC0,C4S6
ADDF S4C5C6,AC0 ;AC0 ← S4*C5*C6 + C4*SIN6
STF AC0,PART2 ;STORE INTERMEDIATE RESULT
LDF C4C5C6,AC0 ;AC0 ← C4*C5*C6
SUBF S4S6,AC0 ;AC0 ← C4*C5*C6 - S4*SIN6
STF AC0,PART1A
MULF C23,AC0 ;AC0 ← C23*PART1A
LDF S23,AC1 ;AC1 ← S23
MULF S5C6,AC1 ;AC1 ← S23*S5*C6
SUBF AC1,AC0 ;AC0 ← C23*PART1A - S23*S5*C6
STF AC0,PART1 ;STORE INTERMEDIATE RESULT
JSR PC,GETXY ;USE PART1 & PART2 TO CALCULATE AC0←NX, AC1←NY
STF AC0,NX(R0) ;STORE NX
STF AC1,NY(R0) ;STORE NY
LDF PART1A,AC0 ;AC0 ← PART1A
MULF S23,AC0 ;AC0 ← S23*PART1A
LDF C23,AC1 ;AC1 ← C23
MULF S5C6,AC1 ;AC1 ← C23*S5*C6
ADDF AC1,AC0 ;AC0 ← S23*PART1A + C23*S5*C6
NEGF AC0 ;NEGATE RESULT ==> -S23*PART1A - C23*S5*C6
STF AC0,NZ(R0) ;STORE RESULT - THIS IS Nz.
RTS PC ;TO CALLER.
; [CONTINUATION OF PUMUPD: GETO]
; Calculate the O vector of the Cartesian solution.
GETO: LDF C5,AC0 ;AC0 ← C5
MULF SIN6,AC0 ;AC0 ← C5*SIN6
STF AC0,AC1 ;AC1 ← C5*SIN6
MULF C4,AC0 ;AC0 ← C4*C5*SIN6
STF AC0,C4C5S6 ;STORE IT
MULF SIN4,AC1 ;AC1 ← S4*C5*SIN6
LDF C4,AC0 ;AC0 ← C4
MULF C6,AC0 ;AC0 ← C4*C6
SUBF AC1,AC0 ;AC0 ← -S4*C5*SIN6 + C4*C6
STF AC0,PART2 ;STORE HALF THE RESULT
LDF SIN5,AC0 ;AC0 ← S5
MULF SIN6,AC0 ;AC0 ← S5*SIN6
STF AC0,S5S6 ;STORE IT
MULF S23,AC0 ;AC0 ← S23*S5*SIN6
STF AC0,PART1B ;STORE IT
LDF SIN4,AC0 ;AC0 ← S4
MULF C6,AC0 ;AC0 ← S4*C6
ADDF C4C5S6,AC0 ;AC0 ← C4*C5*SIN6 + S4*C6
STF AC0,PART1A ;STORE IT
MULF C23,AC0 ;AC0 ← C23*PART1A
NEGF AC0 ;AC0 ← -C23*PART1A
ADDF PART1B,AC0 ;AC0 ← -C23*PART1A + PART1B
STF AC0,PART1 ;STORE OTHER HALF OF RESULT
JSR PC,GETXY ;PUT X & Y COMPONENTS IN AC0 & AC1.
STF AC0,OX(R0) ;STORE X COMPONENT
STF AC1,OY(R0) ;STORE Y COMPONENT
LDF PART1A,AC0 ;AC0 ← PART1A
MULF S23,AC0 ;AC0 ← S23*PART1A
LDF C23,AC1 ;AC1 ← C23
MULF S5S6,AC1 ;AC1 ← C23*S5*SIN6
ADDF AC1,AC0 ;AC0 ← S23*PART1A + C23*S5*SIN6
STF AC0,OZ(R0) ;STORE Z COMPONENT OF O VECTOR
RTS PC ;RETURN
; [CONTINUATION OF PUMUPD: GETA]
; Calculate the A vector of the Cartesian solution.
GETA: LDF C4,AC0 ;AC0 ← C4
MULF SIN5,AC0 ;AC0 ← C4*S5
STF AC0,C4S5 ;STORE IT
LDF SIN4,AC1 ;AC1 ← S4
MULF SIN5,AC1 ;AC1 ← S4*S5
STF AC1,PART2 ;THIS IS HALF THE RESULT
MULF C23,AC0 ;AC0 ← C23*C4*S5
LDF S23,AC1 ;AC1 ← S23
MULF C5,AC1 ;AC1 ← S23*C5
ADDF AC1,AC0 ;AC0 ← C23*C4*S5 + S23*C5
STF AC0,PART1 ;THIS IS THE OTHER HALF
JSR PC,GETXY ;PUT X & Y COMPONENTS IN AC0 & AC1.
STF AC0,AX(R0) ;STORE X COMPONENT
STF AC1,AY(R0) ;AND Y COMPONENT
LDF C23,AC0 ;AC0 ← C23
MULF C5,AC0 ;AC0 ← C23*C5
LDF S23,AC1 ;AC1 ← S23
MULF C4S5,AC1 ;AC1 ← S23*C4*S5
SUBF AC1,AC0 ;AC0 ← -S23*C4*S5 + C23*C5
STF AC0,AZ(R0) ;STORE Z COMPONENT OF A VECTOR
RTS PC
; [CONTINUATION OF PUMUPD: GETP]
; Calculate the P vector of the Cartesian solution.
GETP: LDF D4,AC0 ;AC0 ← D4
MULF S23,AC0 ;AC0 ← D4*S23
LDF A3,AC1 ;AC1 ← A3
MULF C23,AC1 ;AC1 ← A3*C23
ADDF AC1,AC0 ;AC0 ← D4*S23 + A3*C23
LDF A2,AC1 ;AC1 ← A2
MULF COS2,AC1 ;AC1 ← A2*C2
ADDF AC1,AC0 ;AC0 ← D4*S23 + A3*C23 + A2*C2
STF AC0,PART1 ;THIS IS PART OF THE RESULT
MOV D3,PART2 ;SET PART2 ← D3. MOVE 1ST WORD
MOV D3+2,PART2+2 ; AND 2ND WORD.
JSR PC,GETXY ;GET X & Y COMPONENTS, STORE IN AC0 & AC1.
STF AC0,PX(R0) ;STORE X & Y COMPONENTS NOW
STF AC1,PY(R0)
LDF D4,AC0 ;NOW CALCULATE PZ. AC0 ← D4
MULF C23,AC0 ;AC0 ← D4*C23
LDF A3,AC1 ;AC1 ← A3
MULF S23,AC1 ;AC1 ← A3*S23
SUBF AC1,AC0 ;AC0 ← D4*C23 - A3*S23
LDF A2,AC1 ;AC1 ← A2
MULF SIN2,AC1 ;AC1 ← A2*SIN2
SUBF AC1,AC0 ;AC0 ← D4*C23 - A3*S23 - A2*S2
STF AC0,PZ(R0) ;STORE PZ IN RESULT MATRIX
RTS PC
; [CONTINUATION OF PUMUPD: GETXY]
; Support routine.
GETXY: LDF PART1,AC0 ;AC0 ← PART1
MULF COS1,AC0 ;AC0 ← C1*PART1
LDF PART2,AC1 ;AC1 ← PART2
MULF SIN1,AC1 ;AC1 ← SIN1*PART2
SUBF AC1,AC0 ;AC0 ← C1*PART1 - SIN1*PART2
LDF PART1,AC1 ;AC1 ← PART1
MULF SIN1,AC1 ;AC1 ← SIN1*PART1
LDF PART2,AC2 ;AC2 ← PART2
MULF COS1,AC2 ;AC2 ← C1*PART2
ADDF AC2,AC1 ;AC1 ← SIN1*PART1 + C1*PART2
RTS PC
; [CONTINUATION OF PUMUPD: OFSETW]
; Account for the length of the tool by offsetting the result by an amount
; equal to the tool length times the A vector (which is a unit vector).
; For both arms, account for the offset of the origin of the PUMA's
; world, as described in PUMSOL.
; For the Green arm, account for the rotation of the PUMA's world
; by negating the X and Y components of the direction vectors.
; Green transformation: (x,y,z) → (x0-x, y0-y, z0+z).
; Red transformation: (x,y,z) → (x0+x, y0+y, z0+z).
OFSETW: LDF AX(R0),AC0 ;AC0 ← AX
MULF TOOLEN,AC0 ;AC0 ← AX*TOOLLENGTH
ADDF PX(R0),AC0 ;AC0 ← PX + AX*TOOLLENGTH
CMP R5,#GCON ;IS IT THE GREEN ARM? IF SO,
BNE 5$ ;THEN CONTINUE AND NEGATE PX
NEGF AC0 ;NOW OFFSET THE ORIGIN: GET -PX
5$: ADDF XCOORD(R5),AC0 ;FIND +-X + X0
STF AC0,PX(R0) ;THIS IS OFFSET PX
LDF AY(R0),AC0 ;AC0 ← AY
MULF TOOLEN,AC0 ;AC0 ← AY*TOOLLENGTH
ADDF PY(R0),AC0 ;AC0 ← PY + AY*TOOLLENGTH
CMP R5,#GCON ;GREEN ARM?
BNE 10$
NEGF AC0 ;ACCOUNT FOR OFFSET ORIGIN
10$: ADDF YCOORD(R5),AC0
STF AC0,PY(R0) ;THIS IS OFFSET PY
LDF AZ(R0),AC0 ;AC0 ← AZ
MULF TOOLEN,AC0 ;AC0 ← AZ*TOOLLENGTH
ADDF PZ(R0),AC0 ;AC0 ← PZ + AZ*TOOLLENGTH
ADDF ZCOORD(R5),AC0 ;NOW OFFSET TO ORIGIN
STF AC0,PZ(R0) ;THIS IS OFFSET PZ
CMP R5,#GCON ;GREEN ARM?
BNE 20$
NEGF NX(R0) ;NEGATE THE N VECTOR
NEGF NY(R0)
NEGF OX(R0) ;NEGATE THE O VECTOR
NEGF OY(R0)
NEGF AX(R0) ;NEGATE THE Z VECTOR
NEGF AY(R0)
20$: RTS PC
; [CONTINUATION OF PUMUPD: LOCAL DATA AND CONSTANTS]
DATA
C4C5C6: .BLKW 2 ;INTERMEDIATE RESULTS
S4C5C6: .BLKW 2
S5C6: .BLKW 2
S4S6: .BLKW 2
C4S6: .BLKW 2
C5S6: .BLKW 2
S5S6: .BLKW 2
C4C5S6: .BLKW 2
C4S5: .BLKW 2
ANG2T1: .BLKW 2
ANG2T2: .BLKW 2
PART1A: .BLKW 2
PART1B: .BLKW 2
CODE
;JACOB - COMPUTES JACOBIAN MATRIX
;COMPUTES JACOBIAN MATRIX FOR A ARBITRARY TRANSFORMATIION.
;
; WE NEED:
; 1. PTR TO STORAGE FOR JACOBIAN MATRIX
; 2. PTR TO TABLE OF PTRS TO JOINT ANGLES
; 3. PTR TO ORC OR 6RC MATRIX
; 4. BLUE/YELLOW INDICATOR
; 5. ORC OR 6RC INDICATOR
; 6. STORAGE FOR CA6, CA3, CA0?
; R0 ← PTR TO FORCE TRANSFORMATION: C
; R1 ← PTR TO TABLE OF PTRS TO JOINT ANGLES
; R2 ← ARM/TABLE/HAND BITS
; R3 ← PTR TO ARRAY TO RETURN JACOBIAN IN
;ON COMPLETION
; T0 ← CT0 T3 ← CT3 T6 ← CT6
;EXECUTION TIME: 4 MSEC
;REGISTERS USED:
; R0,R1,R2,R3 PASS ARGUMENTS AND R0,R1 ARE GARBAGED
; ["JACOB" - FORM CT0,CT3,CT6]
JACOB: MOV R5,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
MOV R3,-(SP) ;RETURN JACOBIAN IN HERE
MOV R2,-(SP)
MOV #YCON,R3 ;ADDR OF YELLOW ARM PHYSICAL CONSTANTS
BIT #YELARM,R2 ;CHECK IF YELLOW OR BLUE ARM
BNE 1$ ;SKIP IF YELLOW ARM
MOV #BCON,R3 ;ELSE GET ADDR OF BLUE ARM CONSTANTS
ADD #14.,R1 ;POINT TO BLUE ARM JOINT ANGLES
1$: MOV S1+2(R3),-(SP) ;SAVE S1
MOV S1(R3),-(SP)
MOV R0,-(SP) ;SAVE C
BIT #FTABLE,R2 ;FORCE SENSING IN HAND OR TABLE COOR.
BNE 2$
;RELATIVE HAND TRANSFORMATION GIVEN:6TC, COMPUTE 3T0 AND 6T3
JSR PC,T6330 ;T3 ← 6T3 , T6 ← 3T0
MOV #T611,-(SP) ;T6 ← CT6 ← INVERSE(6TC)
MOV #T011,-(SP) ;T0 ← CT0 ← CT3*3T0
BR GET036
;TABLE TRANSFORMATION GIVEN:0TC, COMPUTE 0T3 AND 3T6
2$: MOV #10,R4 ;T3 ← 0T3 , T6 ← 3T6
JSR PC,T0336 ;DONT ADD IN BASEX OR BASEY
MOV #T011,-(SP) ;T0 ← CT0 ← INVERSE(0TC)
MOV #T611,-(SP) ;T6 ← CT6 ← CT3*3T6
;COMPUTE T0←CT0, T3←CT3, T6←CT6
GET036: MOV R0,R1 ;C
; MOV #T311,R0 ;T3←C*T3 *K
; MOV #T311,R2 ;T3 *K
; JSR PC,MATMUL ; *K
;REMOVE FROM HERE TO
MOV #T312,R0 ;T3 ← CT3 ← C*T3
MOV #T312,R2 ;FORM COL 2,3,4 OF NEW T3
MOV #3,R3
JSR PC,MULROW
MOV #T311,R0 ;COMPUTE T3(I,1) BY CROSSING T3(I,2)
MOV #T312,R1 ; WITH T3(I,3)
MOV #T313,R2
JSR PC,CROSS
;TO HERE
MOV (SP)+,R0 ;(R0) ← T3*T6
MOV #T311,R1
MOV #T611,R2
JSR PC,MATMUL
MOV (SP)+,R0 ;(R0) ← INVERSE(C)
MOV (SP)+,R1 ;C
; JSR PC,INVERT ; *K
;REMOVE FROM HERE
MOV #3,R2
1$: MOV (R1)+,(R0)+ ;TRANSPOSE A COL INTO A ROW
MOV (R1)+,(R0)+
MOV (R1)+,10(R0)
MOV (R1)+,12(R0)
MOV (R1)+,24(R0)
MOV (R1)+,26(R0)
SOB R2,1$
ADD #30,R0
CLRF (R0)+ ;X,Y,Z ← 0
CLRF (R0)+
CLRF (R0)
;TO HERE
; ["JACOB" - POSITION VECTORS, AXES OF ROTATION]
;COMPUTE POSITION VECTORS FROM AXES OF ROTATION TO HAND AND DIRECTION
;COSINES OF AXES OF ROTATION, ALL IN COORDINATE SYSTEM C
FRMJAC: LDF (SP)+,AC3 ;NEED S1 TO DETERMINE A POINT ON Z2
MOV 2(SP),R5 ;SAVE JACOBIAN IN HERE
MOV #T614,R0 ;CPX6
MOV #T014,R1 ;CPX0
MOV #T013,R2 ;CAX0
MOV #3,R3
1$: LDF (R0)+,AC0 ;POSITION VECTOR FOR JT 1: P1 ← CP6-CP0
SUBF (R1)+,AC0
STF AC0,(R5)+
LDF (R2),AC1 ; " " " JT 2: P2 ← P1-S1*CA0
MULF AC3,AC1
MOV (R2)+,10(R5) ;ROTATION AXIS OF JT 1: Z1 ← CA0
MOV (R2)+,12(R5)
SUBF AC1,AC0
STF AC0,24(R5)
SOB R3,1$
ADD #30,R5 ;STORE ROTATION AXIS OF JT 2 HERE: Z2
MOV #T311,R0 ;Z2 ← -CT3 ( 1ST COLUMN OF MATRIX CT3 )
MOV #T313,R1 ;JT 3 JACOBIAN HAS ONLY A FORCE COMPONENT: CA3
MOV #3,R2
2$: LDF (R0)+,AC0 ;Z2 ← -CT3
NEGF AC0
STF AC0,(R5)+
LDF (R1)+,AC0 ;CA3
STF AC0,10(R5)
CLRF 24(R5) ;CONTRIBUTION OF JT3 TO ROTATIONS: Z3 ← 0
SOB R2,2$
ADD #30,R5 ;TEMPORARILY SAVE POSITION VECTOR JT 4 IN HERE
MOV #T614,R0 ;CPX6
MOV #T314,R1 ;CPX3
MOV #T313,R2 ;CAX3
MOV #3,R3
3$: LDF (R0)+,AC0 ;POSITION VECTOR FOR JT 4: P4 ← CP6-CP3
SUBF (R1)+,AC0
MOV (R2)+,14(R5) ;ROTATION AXIS OF JT 4: Z4 ← CA3
MOV (R2)+,16(R5)
STF AC0,(R5)+
STF AC0,24(R5) ;POSITION VECTOR FOR JT 5: P5 ← P4
SOB R3,3$
ADD #30,R5 ;PUT DIRECTION COSINES OF Z5 IN HERE
LDF SINT6,AC2 ;NEED SIN/COS THETA 6
LDF COST6,AC3
MOV #T611,R0 ;1ST COL OF CT6
MOV #T612,R1 ;2ND " " "
MOV #T613,R2 ;CAX6
MOV #3,R3
4$: LDF (R0)+,AC0 ;COMPONENT OF Z5 ← T6I1*ST6+T6I2*CT6
MULF AC2,AC0
; MOV 14(R2),14(R5) ;POSITION VECTOR OF JT6: P6 ← CP6 *K
; MOV 16(R2),16(R5) ; *K
CLR 14(R5) ;POSITION VECTOR FOR JT6: P6 ← 0
CLR 16(R5)
LDF (R1)+,AC1
MULF AC3,AC1
MOV (R2)+,30(R5) ;ROTATION AXIS OF JT 6: Z6 ← CAX6
MOV (R2)+,32(R5)
ADDF AC1,AC0
STF AC0,(R5)+
SOB R3,4$
; ["JACOB" - CROSS PRODUCTS, CLEAN UP]
;COMPUTE COMPONENTS OF LINEAR MOTION BY TAKING CROSS PRODUCTS. NOTE
;THAT JOINT 6 DOES NOT CONTRIBUTE TO LINEAR MOTION AND JOINT 3 NEEDS
;NO CROSS PRODUCT SINCE IT IS A PRISMATIC JOINT.
CROSSS: MOV R5,R0 ;POSITION COMPONENTS JT 1: Z1 X P1
SUB #170,R0 ;1ST ROW, 1ST COL OF JACOBIAN
MOV R0,R1
ADD #14,R1 ;Z1
MOV R0,R2 ;P1
JSR PC,CROSS
ADD #20,R0 ;POSITION COMPONENTS JT 2: Z2 X P2
ADD #20,R1 ;Z2
ADD #20,R2 ;P2
JSR PC,CROSS
ADD #50,R0 ;POSITION COMPONENTS JT 4: Z4 X P4
ADD #50,R1 ;Z4
ADD #50,R2 ;P2
JSR PC,CROSS
ADD #20,R0 ;POSITION COMPONENTS JT 5: Z5 X P5
ADD #20,R1 ;Z5
ADD #20,R2 ;P5
JSR PC,CROSS
;RESTORE REGISTERS AND EXIT
MOV (SP)+,R2 ;RESTORE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R4
MOV (SP)+,R5
RTS PC ;EXIT
;END OF "JACOB"
;T0336 - COMPUTES TRANSFORMATION MATRICIES FOR JTS 1-3 AND 4-6
;"T0336" COMPUTES THE TRANSFORMATION MATRIX FROM THE BASE SYSTEM (0)
;TO THE END OF JOINT 3 AND THE MATRIX FROM THE COORDINATE SYSTEM OF
;JOINT 3 TO THE END OF JOINT 6. ON COMPLETION, THE MATRIX FROM 0 TO 3
;WILL BE STORED IN THE ARRAY "T3" AND THE MATRIX FROM 3 TO 6 WILL
;BE STORED IN "T6". IF R4 IS 0 THE X,Y LOCATION OF THE BASE OF THE
;ARM WILL BE ADDED IN, OTHERWISE R4 SHOULD BE SET TO '10
T0336U: MOV #T36PTU,R2 ;TABLE OF PTRS TO T0_3&T3_6
BR .+6
T0336: MOV #T36PTR,R2 ;TABLE OF POINTERS TO T0_3&T3_6
JSR PC,R03 ;CONSTRUCT UPPER LEFT 3x3 OF T0_3
LDF @(R1)+,AC0 ;T0_3(3,4)=CS2*S3+S1
MULF AC0,CS2
ADDF (R3)+,AC1 ;GET S1
STF AC1,T334
MULF AC0,AC2 ;T0_3(1,4)=CS1*SN2*S3-SN1*S2
MULF AC0,AC3 ;T0_3(2,4)=SN1*SN2*S3+CS1*S2
LDF (R3)+,AC1 ;GET S2
STF AC1,AC0
MULF SN1,AC0
ADD R4,R3 ;SKIP BASEX & BASEY IF REQUESTED
TST R4
BNE .+4
ADDF (R3)+,AC2 ;+ BASEX
SUBF AC0,AC2
STF AC2,T314
MULF CS1,AC1
TST R4
BNE .+4
ADDF (R3)+,AC3 ;+ BASEY
ADDF AC1,AC3
STF AC3,T324
JSR PC,R36 ;CONSTRUCT UPPER LEFT 3x3 OF T3_6
STF AC0,AC1 ;T3_6(3,4)=CS5*S6
MULF CS5,AC1
STF AC1,@(R2)+
STF AC0,AC1 ;T3_6(2,4)=SN4*ST5*SIN6
MULF @(R2)+,AC1
STF AC1,@(R2)+
MULF @(R2)+,AC0 ;T3_6(1,4)=CS4*ST5*S6
STF AC0,@(R2)
RTS PC
;END OF "T0336"
;T6330 - COMPUTES TRANSFORMATION MATRICIES FOR JTS 6-4 AND 3-0
;"T6330" COMPUTES THE INVERSE TRANSFORMATION MATRICIES FROM THOSE
;COMPUTED BY "T0336". ON COMPLETION, THE MATRIX FROM JOINT 6 TO 3
;IS STORED IN "T3" AND THE MATRIX FROM 3 TO 0 IS STORED IN "T6".
;NOTE THAT THE CORRECTIONS FOR THE X-Y LOCATION OF THE BASE OF
;THE ARMS IS NOT ADDED INTO THE MATRIX TRANSFORMATIONS.
T6330: MOV #T63PTR,R2 ;TABLE OF POINTERS TO T3_0&T6_3
JSR PC,R03 ;CONSTRUCT UPPER LEFT 3X3 OF T3_0
LDF (R3)+,AC2 ;T3_0(2,4)=SN2*S1
MULF AC2,SN2
MOV (R3)+,T614 ;T3_0(1,4)=S2
STF SN2,T624
MULF AC2,CS2 ;T3_0(3,4)=-CS2*S1-S3
MOV (R3),T614+2
ADDF @(R1)+,CS2
ADD #12,R3 ;DONT ADD IN BASEX & BASEY
NEGF CS2
STF CS2,T634
JSR PC,R36 ;CONSTRUCT UPPER LEFT 3X3 OF T6_3
CLRF T314 ;T6_3(1,4)=0
CLRF T324 ;T6_3(2,4)=0
NEGF AC0 ;T6_3(3,4)=-S6
STF AC0,T334
RTS PC
;END OF "T6330"
;R03 - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 1-3
;ON COMPLETION, THIS ROUTINE WILL HAVE STORED THE ELEMENTS OF R0_3(1:3,1:3)
;INTO THE ARRAY POINTED TO BY R2. IN ADDITION, THE FLOATING POINT REGISTERS
;WILL CONTAIN THE FOLLOWING VALUES:
;
; AC0= SN2 AC1= CS2 AC2= CS1*SN2
; AC3= SN1*SN2 AC4= SN1 AC5= CS1
R03: LDF @(R1)+,AC0 ;COMPUTE SIN/COS THETA 1
JSR PC,SNCOS
STF SN,@(R2)+ ;T0_3(1,1)=SN1
STF CS,@(R2)+ ;T0_3(2,1)=-CS1
STF SN,SN1 ;AND SAVE FOR LATER
STF CS,CS1
LDF @(R1)+,AC0 ;COMPUTE SIN/COS THETA 2
JSR PC,SNCOS
STF SN,@(R2)+ ;T0_3(3,2)=-SN2
STF CS2,@(R2)+ ;T0_3(3,3)=CS2
STF CS2,AC2 ;T0_3(1,2)=CS1*CS2
MULF CS1,AC2
ADD #100000,@-6(R2) ;CORRECT SIGN( T0_3(2,1) )
STF AC2,@(R2)+
STF CS2,AC2 ;T0_3(2,2)=SN1*CS2
MULF SN1,AC2
ADD #100000,@-6(R2) ;CORRECT SIGN( T0_3(3,2) )
STF AC2,@(R2)+
LDF SN2,AC2 ;T0_3(1,3)=CS1*SN2
MULF CS1,AC2
STF AC2,@(R2)+
LDF SN1,AC3 ;T0_3(2,3)=SN1*SN2
MULF SN2,AC3
STF AC3,@(R2)+
CLRF @(R2)+ ;T0_3(3,1)=0
RTS PC
;END OF "R03"
;R36 - COMPUTES ROTATION MATRIX FOR TRANSFORMATION FROM JTS 4-6
;ON COMPLETION, THIS ROUTINE WILL HAVE STORED THE ELEMENTS OF R3_6(1:3,1:3)
;INTO THE ARRAY POINTED TO BY R2. SINCE THE ELEMENT R3_6(1:1) IS NEVER
;NEED BY ANY OF THE CALLING ROUTINES, THIS IS THE ONLY MATRIX ELEMENT THAT
;IS NOT COMPUTED. AT THE COMPLETION OF THIS ROUTINE, THE FLOATING POINT
;REGISTERS CONTAIN THE FOLLOWING VALUES:
; AC0= S6 AC5= CS5
R36: LDF @(R1)+,AC0 ;COMPUTE SIN/COS THETA 4
JSR PC,SNCOS
STF SN,-(SP) ;SAVE SIN/COS
STF CS,CS4
LDF @(R1)+,AC0 ;COMPUTE SIN/COS THETA 5
JSR PC,SNCOS
STF CS,@(R2)+ ;T3_6(3,3)=CS5
STF SN,AC2 ;T3_6(2,3)=SN4*ST5
MULF (SP),AC2
STF AC2,@(R2)+
STF SN,AC2 ;T3_6(1,3)=CS4*ST5
MULF CS4,AC2
STF AC2,@(R2)+
STF SN,-(SP) ;SAVE SIN/COS THETA 5
STF CS,CS5
LDF @(R1)+,AC0 ;COMPUTE SIN/COS THETA 6
JSR PC,SNCOS
STF SN,SINT6 ;JACOB NEEDS THESE
STF CS,COST6
LDF (SP)+,AC2 ;T3_6(3,2)=ST5*ST6
STF AC2,AC3
MULF SN,AC2
STF AC2,@(R2)+
MULF CS,AC3 ;T3_6(3,1)=-ST5*CT6
STF AC3,@(R2)
LDF (SP)+,AC2 ;NEED COMBINATIONS OF SIN/COS T4&T6
STF AC2,AC3
MULF SN,AC2 ;SN4*ST6
ADD #100000,@(R2)+ ;COMPLEMENT T3_6(3,1)
MULF CS,SN4 ;SN4*CT6
MULF CS4,SN ;CS4*ST6
MULF CS4,CS ;CS4*CT6
MULF CS5,AC2 ;T3_6(2,2)=-SN4*CS5*ST6+CS4*CT6
SUBF AC1,AC2
STF AC2,@(R2)
STF AC3,AC2 ;T3_6(2,1)=SN4*CS5*CT6+CS4*ST6
MULF CS5,AC2
ADD #100000,@(R2)+ ;COMPLEMENT T3_6(2,2)
ADDF AC0,AC2
STF AC2,@(R2)+
MULF CS5,AC0 ;T3_6(1,2)=-SN4*CT6-CS4*CS5*ST6
ADDF AC3,AC0
NEGF AC0
STF AC0,@(R2)+
LDF (R3)+,AC0 ;NEXT ROUTINE NEEDS S6
RTS PC
;END OF "R36"
;INVERT - INVERTS A TRANSFORMATION STORED BY COLUMNS
;
; R0 ← PTR TO STORAGE FOR INVERTED TRANSFORMATION
; R1 ← PTR TO TRANSFORMATION TO BE INVERTED
;
INVERT: MOV #3,R2 ;COUNTER
LDF 44.(R1),AC0
STF AC0,AC4
LDF 40.(R1),AC3
LDF 36.(R1),AC2
1$: LDF (R1)+,AC0 ;NEW T(I,4) ← -OLD T(J,I)*T(J,4)
STF AC0,(R0)
MULF AC2,AC0
ADD #12.,R0
LDF (R1)+,AC1
STF AC1,(R0)
MULF AC3,AC1
ADD #12.,R0
ADDF AC1,AC0
LDF (R1)+,AC1
STF AC1,(R0)
MULF AC4,AC1
SUB #20.,R0
ADDF AC1,AC0
NEGF AC0
STF AC0,32.(R0)
SOB R2,1$
RTS PC
;END OF "INVERT"
;MATMUL - PERFORMS THE OPERATION T0 ← T1*T2
;WON'T WORK IF LOC(T0) = LOC(T1)
;
; R0 ← PTR TO ARRAY FOR RESULT: T0
; R1 ← PTR TO T1
; R2 ← PTR TO T2
MATMUL: JSR PC,MULSHR ;FORM COL 2,3,4
SUB #36.,R0 ;COMPUTE T1(I,1) BY CROSSING T1(I,2)
MOV R0,R1 ; WITH T1(I,3)
ADD #12.,R1
MOV R1,R2
ADD #12.,R2
JSR PC,CROSS
RTS PC
MULROT: ADD #12.,R0 ;FORM COL 1,2,3,4 OF TRANS X ROTATION TRANS
ADD #12.,R2
MOV #2,R3
JSR PC,MULCOL
ADD #36.,R1 ;LAST ROW OF T0 = T1
LDF (R1)+,AC0
STF AC0,(R0)+
LDF (R1)+,AC0
STF AC0,(R0)+
LDF (R1),AC0
STF AC0,(R0)
SUB #44.,R0 ;COMPUTE T1(I,1) BY CROSSING T1(I,2)
MOV R0,R1 ; WITH T1(I,3)
ADD #12.,R1
MOV R1,R2
ADD #12.,R2
JSR PC,CROSS
RTS PC
MULSHR: ADD #12.,R0 ;FORM COL 2,3,4 OF TRANS X TRANS
ADD #12.,R2
MOV #3,R3
JSR PC,MULCOL
ADD #44.,R1 ;ADD IN T1(I,4)
ADDF (R1),AC0
STF AC0,-(R0)
LDF -(R0),AC0
ADDF -(R1),AC0
STF AC0,(R0)
LDF -(R0),AC0
ADDF -(R1),AC0
STF AC0,(R0)
RTS PC
MULR: JSR PC,MULRSH ;FORM COL 1,2,3 OF TRANS X TRANS
SUB #36.,R0 ;COMPUTE T1(I,1) BY CROSSING T1(I,2)
MOV R0,R1 ; WITH T1(I,3)
ADD #12.,R1
MOV R1,R2
ADD #12.,R2
JSR PC,CROSS
RTS PC
MULRSH: ADD #12.,R0 ;FORM COL 2,3 OF TRANS X TRANS
ADD #12.,R2
MOV #2,R3
JSR PC,MULCOL
RTS PC
;END OF "MATMUL"
;MULT - PERFORMS THE OPERATION A0 ← A1*A2
;WILL NOT WORK FOR THE CASE OF LOC(A0)=LOC(A1)
;
; R0 ← PTR TO ARRAY FOR RESULT: A0
; R1 ← PTR TO 3X3 MATRIX: A1
; R2 ← PTR TO ARRAY A2
; R3 ← NUMBER OF COLUMNS IN A0,A2
;
;AT THE END OF THE OPERATION R0,R2 ARE LEFT POINTING AT
;THE FIRST ELEMENT IN THE N+1 COLUMN
;AND AC0 CONTAINS THE LAST SUM. R1 IS LEFT UNCHANGED
MULROW: MOV #BYROWS,R4
BR MULT
MULCOL: MOV #BYCOLS,R4
MULT: MOV #3,R5 ;MULT A2 BY 3 ROWS OF A1
LDF (R2)+,AC2 ;LOAD A COLUMN OF A2
LDF (R2)+,AC3
LDF (R2)+,AC0
STF AC0,AC4
1$: LDF (R1),AC0 ;A1(I,1)*A2(1,J)
MULF AC2,AC0
ADD (R4)+,R1
LDF (R1),AC1 ;+ A1(I,2)*A2(2,J)
MULF AC3,AC1
ADD (R4)+,R1
ADDF AC1,AC0
LDF (R1),AC1 ;+ A1(I,3)*A2(3,J)
MULF AC4,AC1
ADD (R4)+,R1
MOV (R4),R4
ADDF AC1,AC0
STF AC0,(R0)+ ;SAVE A0(I,J)
SOB R5,1$
SOB R3,MULT
RTS PC
DATA
BYCOLS: .WORD 14,14,-24,.+2
.WORD 14,14,-24,.+2
.WORD 14,14,-40,BYCOLS
BYROWS: .WORD 4,4,4,.+2
.WORD 4,4,4,.+2
.WORD 4,4,-40,BYROWS
CODE
;END OF "MULT"
;CROSS - VECTOR CROSS PRODUCT OPERATION V0 ← V1 X V2
; R0 ← PTR TO VECTOR FOR RESULT: V0
; R1 ← PTR TO VECTOR V1
; R2 ← PTR TO VECTOR V2
CROSS: LDF (R1)+,AC0 ;X1
LDF (R2)+,AC1 ;X2
LDF (R2)+,AC2 ;Y2
STF AC2,AC4
LDF (R1)+,AC3 ;Y1
STF AC3,AC5
MULF AC0,AC2 ;X1*Y2
MULF AC1,AC3 ;X2*Y1
SUBF AC3,AC2 ;Z0 = X1*Y2-X2*Y1
LDF (R2),AC3 ;Z2
MULF AC3,AC0 ;X1*Z2
MULF AC5,AC3 ;Y1*Z2
STF AC0,AC5
LDF (R1),AC0 ;Z1
MULF AC0,AC1 ;Z1*X2
MULF AC4,AC0 ;Z1*Y2
SUBF AC5,AC1 ;Y0 = Z1*X2-X1*Z2
SUBF AC0,AC3 ;X0 = Y1*Z2-Z1*Y2
STF AC3,(R0)+ ;SAVE X0,Y0,Z0
STF AC1,(R0)+
STF AC2,(R0)
RTS PC
;END OF "CROSS"
;LOCAL STORAGE AREA
DATA
T36PTU: .WORD T311,T321,T332,T333,T312,T322,T313,T323,T331
.WORD T033,T023,T013,T032,T031,T022,T021,T012,T034
.WORD T023,T024,T013,T014
T36PTR: .WORD T311,T321,T332,T333,T312,T322,T313,T323,T331
.WORD T633,T623,T613,T632,T631,T622,T621,T612,T634
.WORD T623,T624,T613,T614
T63PTR: .WORD T611,T612,T623,T633,T621,T622,T631,T632,T613
.WORD T333,T332,T331,T323,T313,T322,T312,T321
T011: .BLKW 2
T021: .BLKW 2
T031: .BLKW 2
T012: .BLKW 2
T022: .BLKW 2
T032: .BLKW 2
T013: .BLKW 2
T023: .BLKW 2
T033: .BLKW 2
T014: .BLKW 2
T024: .BLKW 2
T034: .BLKW 2
T311: .BLKW 2
T321: .BLKW 2
T331: .BLKW 2
T312: .BLKW 2
T322: .BLKW 2
T332: .BLKW 2
T313: .BLKW 2
T323: .BLKW 2
T333: .BLKW 2
T314: .BLKW 2
T324: .BLKW 2
T334: .BLKW 2
CT6:
T611: .BLKW 2
T621: .BLKW 2
T631: .BLKW 2
T612: .BLKW 2
T622: .BLKW 2
T632: .BLKW 2
T613: .BLKW 2
T623: .BLKW 2
T633: .BLKW 2
T614: .BLKW 2
T624: .BLKW 2
T634: .BLKW 2
SINT6: .BLKW 2 ;SIN THETA 6
COST6: .BLKW 2 ;COS THETA 6
;PHYSICAL CONSTANTS FOR YELLOW AND BLUE ARMS
;ADDRESSES OF ARM CONSTANTS RELATIVE TO THE START OF THE FOLLOWING TABLES
S1 ==0 ;JOINT 1 OFFSET
S2 ==4 ;JOINT 2 OFFSET
BASEX ==10 ;TABLE COORDINATES OF BASE OF ARM
BASEY ==14
S6 ==20 ;JOINT 6 OFFSET
S22 ==24 ;S2**2
STOP1 ==30 ;MIN. & MAX. JOINT STOPS RELATIVE TO START OF CONST. AREA
;JOINT 2 HAS NO STOP LIMIT SINCE ALL SOLUTIONS ARE POSSIBLE
STOP3 ==40
STOP4 ==50
STOP5 ==60
STOP6 ==70
MIDDY1==100 ;MID-RANGE OF MOTION
MIDDY4==104
; [YELLOW ARM TABLE OF CONSTANTS]
YCON: .FLT2 16.24 ;JOINT 1 OFFSET
.FLT2 6.05 ;JOINT 2 OFFSET
.FLT2 29.5 ;TABLE COORDINATES OF ARM BASE
.FLT2 8.375
.FLT2 9.38 ;JOINT 6 OFFSET
.FLT2 36.6025 ;S2**2
;YELLOW ARM JOINT STOP LIMITS
.FLT2 -185.0 ;JOINT 1 MIN
.FLT2 60.0 ; MAX
.FLT2 6.5 ;JOINT 3 MIN
.FLT2 27.5 ; MAX
.FLT2 -175.0 ;JOINT 4 MIN
.FLT2 86.0 ; MAX
.FLT2 -101.0 ;JOINT 5 MIN
.FLT2 101.0 ; MAX
.FLT2 -135.0 ;JOINT 6 MIN -290.0 WITH BOTH WIPERS WORKING
.FLT2 110.0 ; MAX
.FLT2 -62.5 ;MIDDY1
.FLT2 -44.5 ;MIDDY4
; [BLUE ARM TABLE OF CONSTANTS]
;CONSTANT PARAMETERS FOR BLUE ARM, **ORDERED LIST**
BCON: .FLT2 20.24 ;JOINT 1 OFFSET
.FLT2 -6.05 ;JOINT 2 OFFSET
.FLT2 29.53125 ;TABLE COORDINATES OF ARM BASE
.FLT2 50.805
.FLT2 10.28125 ;JOINT 6 OFFSET
.FLT2 36.6025 ;S2**2
;BLUE ARM JOINT STOP LIMITS
.FLT2 -45.0 ;JOINT 1 MIN
.FLT2 190.0 ; MAX
.FLT2 6.75 ;JOINT 3 MIN
.FLT2 33.0 ; MAX
.FLT2 -395.0 ;JOINT 4 MIN
.FLT2 205.0 ; MAX
.FLT2 -95.0 ;JOINT 5 MIN
.FLT2 95.0 ; MAX
.FLT2 -110.0 ;JOINT 6 MIN
.FLT2 200.0 ; MAX
.FLT2 72.5 ;MIDDY1
.FLT2 -95.0 ;MIDDY4
[GREEN & RED ARM TABLE OF CONSTANTS]
;THESE ARE THE LOCATIONS, IN AL'S TERMS, OF THE ORIGIN OF THE SPACE THAT
;THE PUMAS USE; IE IT IS THE LOCATION OF (0,0,0) OF THE PUMA.
GCON: .FLT2 84.0 ;X COORD OF BASE
.FLT2 52.0 ;Y COORD OF BASE
.FLT2 27.0 ;Z COORD OF BASE
RCON: .FLT2 84.0 ;X COORD
.FLT2 7.0 ;Y
.FLT2 27.0 ;Z
;PUMA CONSTANTS AND MISC. DATA FOR PUMA ROUTINES
; -- CONSTANTS --
A2: .FLT2 17.0 ;THIS IS A[2]
A3: .FLT2 0.0
D3: .FLT2 5.87 ;THIS IS D[3] IN WRITEUP
D4: .FLT2 17.0
TOOLEN: .FLT2 6.7 ;LENGTH OF TOOL
LOSTOP: .FLT2 -160.0, -223.0, -52.0, -2.5, -100.0, -250.0
HISTOP: .FLT2 160.0, 43.0, 232.0, 2.5, 100.0, 250.0
LOLIM: .FLT2 -180.0, -270.0, -90.0, -180.0, -180.0, -180.0
; -- WORK VARIABLES --
PART1: .BLKW 2
PART2: .BLKW 2
SIN1: .BLKW 2 ;SIN(THETA1)
COS1: .BLKW 2 ;COS(THETA1)
SIN2: .BLKW 2 ;SIN(THETA2)
COS2: .BLKW 2 ;COS(THETA2)
SIN3: .BLKW 2 ;SIN(THETA3)
C3: .BLKW 2 ;COS(THETA3)
SIN4: .BLKW 2 ;SIN(THETA4)
C4: .BLKW 2 ;COS(THETA4)
SIN5: .BLKW 2 ;SIN(THETA5)
C5: .BLKW 2 ;COS(THETA5)
SIN6: .BLKW 2 ;SIN(THETA6)
C6: .BLKW 2 ;COS(THETA6)
S23: .BLKW 2 ;SIN(THETA2+THETA3)
C23: .BLKW 2 ;COS(THETA2+THETA3)
; FLOATING POINT CONSTANTS
FLT01: .FLT2 0.01
FLT001: .FLT2 0.001
FLT1: .FLT2 1.0
FLT4: .FLT2 4.0
FLT90: .FLT2 90.0
FLT179: .FLT2 179.5
FLT180: .FLT2 180.0
FLM180: .FLT2 -180.0
FLT360: .FLT2 360.0
TINYNO: .FLT2 -0.0002
CODE